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ABSTRACT 



A problem arises when conventional kinematic equations 
that minimize computational time are used to model a rigid 
revolute robot arm. Mathematical singularities result when 
successive link axes "line up" such that their angles are 0 
or 180 degrees. This may result in erratic and 
uncontrollable motion of the arm until it moves away from 
the point of singularity. One solution is to spend a 
minimum amount of time at the singular position or to avoid 
it altogether. Another solution is to use other sets of 
equations, instead of the regular resolved-rate equations, 
to model the robot arm. This thesis shows how using 
equations based on Newton's Second Principle of dynamics 
for a three link, two degree of freedom manipulator, the 
problem of singularity is avoided. The equations are 
demonstrated in a simulation program. 
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TABLE OF SYMBOLS AND ABBREVIATIONS 



COMPUTER TEXT DESCRIPTION 

SYMBOL VARIABLE 



A 


A 


Sine wave input torque data 
amplitude 


AA 


Aa 


Acceleration of point a 


AB 


Ab 


Acceleration of point b 


AG1 


Agl 


The acceleration vector of 
the center of gravity for 
link 1 


AG 2 


kg 2 


Same as Agl but for link 2 


AG 3 


Ag3 

A-A 


Same as Agl but for link 3 


AOX 


aox 


Linear acceleration of link 
zero in the x direction 


AOY 


aoy 


Linear acceleration of link 
zero in the y direction 


AOZ 


aoz 


Linear acceleration of link 
zero in the z direction 


AX1 


axl 


Linear acceleration of link 
1 in the x direction 


AY1 


ayl 


Linear acceleration of link 
1 in the y direction 


AZ1 


azl 


Linear acceleration of link 
1 in the z direction 
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COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


AX2 


ax2 


AY2 


ay2 


AZ2 


az2 


AX 3 


ax 3 


AY3 


ay 3 


AZ3 


az3 


CTHETX ( 3 ) 


cdx 



CTHETY ( 3 ) 


c0y 


CTHETZ ( 3 ) 


cdz 


DEGRA 




ETHETXC3) 


6x 


ETHETY( 3) 


0y 


ETHETZ( 3) 


6z 



DESCRIPTION 



Linear acceleration of link 
2 in the x direction 

Linear acceleration of link 
2 in the y direction 

Linear acceleration of link 

2 in the z direction 

Linear acceleration of link 

3 in the x direction 

Linear acceleration of link 
3 in the y direction 

Linear acceleration of link 
3 in the z direction 

A 1x3 vector Cartesian 
value of the angle theta 
for link 1-3 in the x 
direction, results from 
taking the integral of 
angular acceleration in the 
x direction twice, in 
degrees 

Same as c6x but in the y 
direction 

Same as cdz but in the z 
direction 

Conversion from degrees to 
radians 

A 1x3 vector of euler 
angles for link 1-3 in thex 
direction, in degrees 

Same as 8x but in the y 
direction 

Same as Ox but in the z 
direction 



8 



COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


EULORYO) 


th0y3 


Theoretical Euler angle for 
link 3 in the y direction, 
in degrees 


ERROR ( 3 ) 


Error ( 3 ) 


% error between thdy3 and 
0y for the third link in 
the y direction 


ERRT1X 


Errtlx 


% error between computed 
and input value of torque 
at joint 1 


ERRT2X 


Errt2x 


Same as Errtlx but at joint 
2 


FXO 


Fxo 


Computed force in the x 
direction at joint 0 


FYO 


Fyo 


Computed force in the y 
direction at joint 0 


FZO 


Fzo 


Computed force in the z 
direction at joint 0 


FX1 


Fxl 


Computed force in the x 
direction at joint 1 


FY1 


Fyl 


Computed force in the y 
direction at joint 1 


FZ1 


Fzl 


Computed force in the z 
direction at joint 1 


FX2 


Fx2 


Computed force in the x 
direction at joint 2 


FY2 


Fy2 


Computed force in the y 
direction at joint 2 


FZ2 


Fz2 


Computed force in the z 
direction at joint 2 


G 


9 


Gravitational constant 
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COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


HDX ( 2 ) 


HDx 


The time rate o£ change of 
angular momentum of a 2 
element composite body in 
the x direction 


HDY ( 2 ) 


HDY 


Same as HDx but in the y 
direction 


HOZ ( 2 ) 


HDz 


Same as HDx but in the z 
direction 


I 




Counter 


IA 




Row dimension of matrix A 
and matrix B used in LEQT2F 
subroutine 


IER 




Error parameter used in 

subroutine LEQT2F 


IDGT 




Accuracy test used in 
subroutine LEQT2F, for 
iterative improvement 


IXX<3,2) 


Ixx 


A 3x2 matrix of Moment of 
Inertia for the two element 
composite body of link 1-3 
about the x axis 


I YY( 3, 2 ) 


iyy 


Same as Ixx but about the y 
axis 


IZZ ( 3, 2 ) 


Izz 


Same as Ixx but about the z 
axis 


IXZ(3,2) 


Ixz 


A 3x2 matrix of Products of 
Inertia for the two element 
composite body of link 1-3 
about the xz coordinate 
axes 


IXY( 3, 2 ) 


Ixy 


Same as Ixz but for the xy 
axes 


IYZ( 3, 2 ) 


Iyz 


Same as Ixz but for the yz 
axes 
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COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


IXXA (3,2) 


Ixxa 


Theoretical Moment of 

inertia for link 3 about 
joint 2 


JXO 


jxo 


Location of joint 0 in the 
x direction 


JYO 


o 

>1 

•r-> 


Location of joint 0 in the 
y direction 


JZO 


j zo 


Location of joint 0 in the 
z direction 


JX1 


jxl 


Location of joint 1 in the 
x direction 


JY1 


jyi 


Location of joint 1 in the 
y direction 


JZ1 


jzl 


Location of joint 1 in the 
z direction 


JX2 


jx2 


Location of joint 2 in the 
x direction 


JY2 


jy2 


Location of joint 2 in the 
y direction 


JZ2 


j z2 


Location of joint 2 in the 
z direction 


L( 3, 2 ) 


L( 3, 2 ) 


A 3x2 matrix that is the 
distance from center of 
link to center of mass at 
each link end 


LCOGX( 3 ) 


LCOGx 


A 1x3 location of center of 
gravity vector for link 1-3 
in the x direction 


LCOGY ( 3 ) 


LCOGy 


Same as LCOGx but for the y 
direction 


LCOGZ ( 3 ) 


LCOGz 


Same as LCOGx but for the z 
direction 
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COMPUTER 

SYMBOL 

M 

MASS ( 3,2) 

MASS1 

MASS2 

MASS3 

MATA( 27,27) 
MATB( 27 ) 



TEXT 

VARIABLE 

Mass( 3,2) 

Ml 

M2 

M3 

MatA 

MatB 



MI 



MJ 



MK 



MI AO, MJAO and 
MKAO 



MIBO, MJBO and 
MKBO 



DESCRIPTION 



Used in LEQT2F subroutine 
as number o£ right hand 
sides 

A 3x2 matrix o£ mass of 
each element that make up 
the composite body for link 
1-3 

Total mass o£ link 1 

Total mass of link 2 

Total mass o£ link 3 

A 27x27 matrix consisting 
of coefficients of the 
unknown variables 

A 1x27 vector consisting of 
the coefficient of known 
variables on input to 
subroutine LEQT2F and an 
output the solution to the 
linear equations 

Results from subroutine 
CPROD, i component of 
vector cross product 

j component of vector 
cross product 

k component of vector 
cross product 

Cross product between wdl 
and RB/G1 at joint 0, 
link 1, in the x, y, z 
direction 

Cross product between wl 
and RB/G1 at joint 0, link 
1, in the x, y, z direction 
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COMPUTER 

SYMBOL 



TEXT 

VARIABLE 



DESCRIPTION 



MICO, MJCO and 
MKCO 


Cross product between wl 
and MIBO, MJBO and MKBO at 
joint 0, link 1, in the x, 
y, z direction 


MIA1, MJA1 and 
MKA1 


Cross product between wdl 
and RA/G1 at joint 1, link 
1, in the x, y , z direction 


MIB1, MJB1 and 
MKB2 


Cross product between wl 
and RA/G1 at joint 1, link 
1, in the x, y, z direction 


MIC1, MJC1 and 
MIC1 


Cross product between wl 
and MIB1, MJB1 and MKB1 
respectively at joint 1, 
link 1, in the x, y, z 
direction 


MIA2, MJA2 and 
MKA2 


Cross product between wd2 
and RB/G2 at joint 1, link 
2, in the x , y, z direction 


MIB2, MJB2 and 
MKB2 


Cross product between w2 
and RB/G2 at joint 1, link 
2, in the x, y, z direction 


MIC2, MJC2 and 
MKC2 


Cross product between w2 
and MIB2, MJB2 and MKB2 
respectively at joint 1, 
link 2, in the x, y, z 
direction 


MI A3, MJA3 and 
MJA3 


Cross product between wd2 
and RA/G2 at joint 2, link 
2, in the x, y, z direction 


MIB3, MJB3 and 
MKB3 


Cross product between w2 
and RA/G2 at joint 2. link 
2, in the x, y, and z 
direction 
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COMPUTER TEXT 

SYMBOL VARIABLE 

MIC3, MJC3 and 
MKC3 



MIA4, MJA4 and 
MKA4 



MIB4, MJB4 and 
MKB4 



MIC4, MJC4 and 
MKC4 



N 

P 

RUN 

RX( 3, 2 ) Rx ( 3, 2 ) 

RY( 3, 2 ) Ry<3,2) 

RZ( 3, 2 ) Rz(3,2) 



DESCRIPTION 



Cross product between w2 
and MIB3, MJB3 and MKB3 
respectively at joint 2 , 
link 2 , in the x, y, z 
direction 

Cross product between wd3 
and RA/G3 at joint 2 , link 
3, in the z, y, z direction 

Cross product between w3 
and RA/G3 at joint 2 , link 
3, in the x, y, z direction 

MJC4, MJC4 and Cross 
product between w3 and 
MIB4, MJB4, and MKB4 
respectively at joint 2 , 
link 3, at the x, y, z 
direction 

Used in LEQT2F subroutine 
for the order o£ Mat A and 
number of rows of vector B 

Phase angle of sine wave 
input to joints 

Number of the run 

conducting 

A 3x2 matrix consisting of 
the distance from the 
center of gravity of the 
link to center of mass for 
the elements of link 1-3 in 
the x direction 

Same as Rx(3,2) but in the 
y direction 

Same as Rx(3,2) but in the 
z direction 
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COMPUTER 

SYMBOL 

RAG1<3) 

RBG1(3) 

RAG2 ( 3 ) 

RBG2 ( 3 ) 

RBG3 ( 3 ) 

SUMHDX ( 3 ) 

SUMHDY ( 3 ) 
SUMHDZ ( 3 ) 
THETXR( 3 ) 

THETYR( 3 ) 

THETZR( 3) 

TOX, TOY 
TOZ 



TEXT DESCRIPTION 

VARIABLE 



ra/Gl 



rb/Gl 



ra/G2 



rb/G2 



rb/G3 



EHDx 



EHDy 



A 1x3 vector, distance of 
point a to center of 
gravity for link 1, in the 
x, y, z direction 

A 1x3 vector, distance of 
point b to center of 
gravity for link 1, in the 
x, y, z direction 

A 1x3 vector, distance of 
point a to CoG for link 2, 
in the x,y,z direction 

A 1x3 vector, distance of 
point b to CoG for link 2, 
in the x, y, z direction 

A 1x3 vector, distance of 
point b to CoG for link 3, 
in the x, y, z direction 

A 1x3 vector, sum of HDX 
for the two elements of 
link 1-3 in the x direction 

Same as EHDx but in the y 
direction 



ZHDz Same as EHDx but in the z 

direction 

A 1x3 vector of euler 
angles in the x direction 
in radians for link 1-3 

Same as THETXR( 3) but in 
the y direction 

Same as THETXRt 3 ) but in 
the z direction 

Tox, Toy, Input torque at joint 0 at 

Toz the x, y, z direction 
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COMPUTER 


TEXT 




SYMBOL 


VARIABLE 


T1X, T1Y 


Tlx, 


Tly 


T1Z 


Tlz 




T2X, T2Y 


T2x, 


T2y 


T2Z 


T2z 





THDDOTO) 

THEORY ( 3 ) 

THEXR1, THEXR2, 

THEXR3 

THEYR1, THEYR2, 

THEYR3 

THEZR1, THEZR2, 

THEZR3 

T0RY1X Torylx 

TORY2X Tory2x 

TX1, TX2, TX3 

TY1, TY2, TY3 

TZ1, TZ2, TZ3 

VECTAO ( 3 ) and 
VECTBO ( 3 ) 



DESCRIPTION 



Input torque 


at joint 


1 at 


the x, y, z 


direction 




Input torque 


at joint 


2 at 


the x, y, z 


direction 




Theoretical 


value of 


wdx 


for link 3 in degrees 




Theoretical 


value of 


wdx 



for linlc 3 in radians 



Second integral 


of 


wdx 


for 


links 1-3 


in radians 




Second integral 


of 


wdy 


for 


links 1-3 


in radians 




Second integral 


of 


wdz 


for 


link 1-3 


in radians 






Computed 


value 


of 


torque 


for joint 


1 








Computed 


value 


of 


torque 



for joint 2 

Euler angle theta converted 
to radians for links 1-3 
respectively in the x 
direction 

Euler angles theta 

converted to radians for 
links 1-3 respectively in 
the y direction 

Euler angles theta 

converted to radians for 
links 1-3 respectively in 
the z direction 

1x3 vector used in 
subroutine CPROD for joint 
0 
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COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


VECTAK3) and 
VECTB1 ( 3 ) 




1x3 vector used in 
subroutine CPROD for joint 
1 


VECTA2 ( 3 ) and 
VECTB2 ( 3 ) 




1x3 vector used in 
subroutine CPROD for joint 
2 


VECTA ( 3 ) and 
VECTB( 3 ) 




1x3 vector used in 

subroutine CPROD 


W 


w 


Frequency of sine function 
input 


HI, H2, and 

H3 


HI, H2, H3 


Heights of link 1, 2, and 3 


HX( 3 ) 


wx( 3 ) 


A 1x3 vector of angular 
velocity of link 1-3 in the 
x direction 


HY( 3 ) 


wy ( 3 ) 


Same as wx(3) but in the y 
direction 


HZ ( 3 ) 


wz ( 3 ) 


Same as wx(3) but in the z 
direction 


HDX( 3 ) 


wdx ( 3) 


Angular acceleration of 

link 1-3 in the x direction 


HDY( 3) 


vdy ( 3 ) 


Angular acceleration of 

link 1-3 in the y direction 


HDZ ( 3 ) 


vdz ( 3 ) 


Angular acceleration of 

link 1-3 in the z direction 


HKAREA 




Hork area for LEQT2F 

subroutine 


XI, X2 and 
X3 




Location of center of 
gravity for link 1-3 in the 
x direction 


Y 


y 


Theoretical value of y 



distance from Fz2 to center 
of gravity of link 3 
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COMPUTER 

SYMBOL 


TEXT 

VARIABLE 


DESCRIPTION 


Yl, Y2 and 
Y3 




Location of center of 
gravity for link 1-3 in the 
y direction 


Z 


z 


Theoretical value of z 
distance from Fy2 to center 
of gravity of link 3 


Zl, Z2 and 
Z3 




Location of center of 
gravity for link 1-3 in the 
z direction 
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I. SUTKQP P CT I Q N 



Manipulator models which use local coordinates as a 
basis for simulation and control have a mathematical 
singularity built into them (Ref. 1]. This singularity 
occurs when rigid robot links align such that their 
relative position is either 0° or 180°. When this happens, 
the inverse of the Jacobian matrix becomes impossible to 
compute and the forward dynamics solution cannot be found. 

In the control of serial link manipulators there have 
been various approaches which use local coordinates to 
achieve computational efficiency. One method deals with the 
Newton-Euler approach (Refs. 2, 3], another uses the 
Lagrangian approach (Refs. 4, 5] or there is the method of 
virtual work (Ref. 6). Still another that has tried to make 
the solution to the dynamic equations computationally 
efficient by using Kane's Dynamical equations (Ref. 7]. 
However, although these methods have been computationally 
efficient, they have not been able to handle the problem of 
singularity [Ref. 1]. 
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Various methods have been proposed to deal with the 
problem of singularity. One such method is to minimize the 
time near the singularity [Ref. 8], thereby reducing its 
effects. Another solution is to avoid the position of the 
manipulator that caused the singularity [Refs. 9, 10]. 
However, when using resolved rate equations the arm may 
pass through a point of singularity anyway, in response to 
a command [Ref. 11]. Nakamura and Hanafusa [Ref. 12] have 
proposed to determine the joint motion for the requested 
motion of the end effector by evaluating the feasibility of 
the joint motion. This determined joint motion is called an 
inverse kinematic solution with singularity robustness. 
Other solutions deal with presenting equations that can 
translate the manipulators in the neighborhood of 
singularity [Refs. 13, 14] and in identifying geometric 
singular positions [Ref. 15]. 

It has also been shown that redundancy of robot 
manipulators is effective in dealing with singularities 
[Ref. 16]. Klein and Huang [Ref. 17] have studied the 
method of pseudo-inverse control, for use with redundant 
manipulators, with recommendations for improvement. 
Uchiyama [Ref. 18] proposed switching the control mode in 
the neighborhood of singular points from the mode using 
inverse kinematics to the joint control mode. A seven 
degree of freedom kinematic design with a spherical 
shoulder joint was proposed [Refs. 19, 20], as well as a 
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seven joint robot [Ref. 21] to handle singularity. A four 
degree of freedom wrist was studied to overcome wrist 
singularity [Ref. 22]. Shih [Ref. 23] looked at the 
physical quantities and combinations of physical quantities 
which are unaffected by redundancy to simplify the 
solution of a redundant system. However, even though there 
are some redundant manipulators constructed [Refs. 24, 25], 
research cannot do away with singularities, and so 
consideration still has to be given to the control of the 
manipulator in case of inadvertant singularities. 

This paper will derive equations of motion using the 
First Principles of Newtonian dynamics in terms of global 
coordinates in order to eliminate the problem of 
singularity. By the method of free body analysis, each link 
of the manipulator is treated as if it were a free body 
with forces and moments applied at the joints. Only 
revolute joints will be considered. Although tedious and 
time consuming (computer time), this paper will show by 
simulation how the problem of singularity may thus be 
overcome . 
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II. ROBOT HQDEI. MMG AND SIMULATION PROBLEM 



This thesis does not deal with the control aspect of 
rigid, revolute linkages but rather the mathematical 
dynamic modelling. Given the dynamic model, the link masses 
and inertia properties, initial link alignments, and joint 
torques, then the joint forces, acceleration, velocity and 
position can be predicted via a simulation program. In the 
present approach, all dynamic properties except for 
acceleration and forces were assumed to remain constant 
over a simulated time interval. This assumption linearized 
the equation of motion so that a simple matrix inversion 
could be used to solve for the unknowns. As shown in Figure 
1, the simulation is updated with the predicted velocity 

t 

(£) and position (g.), following integration at the next 
time step. Simulation validation is done by comparing the 
theoretical position (th8y3) to the predicted position 
(0y3) for link 3 and actual torque (Tlx, T2x) to computed 
torque (Torylx, Tory2x) for links 2 and 3. 
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Fipure 1. Manipulator Simulation Block Piapram 



23 



III. THEORETICAL DEVELOPMENT 



A. MANIPULATOR ARM CONFIGURATION 

This thesis develops a generalized simulation program 
for a robot manipulator that is a serial connection of 
three rigid links, jointed by one-degree of freedom 
revolute joints. Joint actuators are assumed to be located 
between successive links to apply the torque necessary to 
position the link. 

B. THEORY 

The method of solution is based on the principle of 
free body analysis. For this approach each body of the 
three link manipulator is treated as if it were a free body 
with forces and moments applied at each joint, as shown in 
Figure 2. The global cartesian coordinate system X, Y, Z as 
well as force and moment torque conventions are also 
evident in the figure. Note that a local coordinate system, 
that is a coordinate system that is local to each joint, 
will not be used but rather a single global system will be 
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Figure 2. Free Body Diagrams 
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will be 



adopted. So all positions, distances, etc., 
referenced to the base of the manipulator system which will 
be at joint zero. The effects of flexibility of the robot 
manipulator will not be considered since ideal, rigid 
bodies are assumed. 

In developing the dynamic equations of motion for each 
link Newton's Second Principle of Notion is used. The known 
variables are torque at the joints, mass of each link, 
linear acceleration of joint zero, initial angular 
acceleration, angular velocity and position of all links. 
The unknowns are the forces at the joints, linear and 
subsequent angular accelerations of the links. 

C. DYNAMIC EQUATIONS OF MOTION OF LINK ONE 
1. Sum of Forces Equations 

In the free body analysis of link one (Figure 2) the 
sum of the forces in the x direction is: 



£Fx=Fxl - FxO= Mlaxl 



(1) 



Similarly sum of the forces in the y direction is 



£Fy=Fyl - FyO=Mlayl 



(2) 



and the sum of the forces in the z direction is 



EFz=Fzl - FzO - Wl=Mlazl 



(3) 
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2. Joint Eq uations 



lie begin by evaulating the joint equations at joint 
zero (Ref. 26, equation (8/4), pp. 423). If the joint is 
sequested and analysis conducted at a point on link zero 
(subscript a) and another at a point on link one (subscript 
b) that is connon to both, so when linked together they are 
equal. This results in two equations and the two unknowns 
wdl and wl. 

As a result: 

Aa = Ao 

which is the acceleration at joint zero, 
and 

Ab = A1 + ( wdl X rb/Gl ) + wl X (wl X rb/Gl) 
which is the acceleration of point b on joint one. Here 
rb/Gl is the distance from point b to the center of gravity 
of link one, and A1 is the acceleration at the center of 
mass of link one or, 

rb/Gl=( jxO-LCOGxl ) i + ( jyO-LCOGyl ) j + ( jzO-LCOGzl )k 
=rb/Glx + rb/Gly + rb/Glz 

After equating Aa and Ab and having the known variables on 
the right side of the equation and unknown variables on the 
left side the following sets of equations result: 

Axl + wdyl( rb/Glz )-wdzl(rb/Gly)= Aox-MICO (4) 

where HICO equals 
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=wylwxl( rb/Gly ) -w'yl (rb/Glx) -w a zl( rb/Glx) 

+ wzlwxl ( rb/Glz ) 

also 

Ayl +wdzl (rb/Glx )-wdxl( rb/Glz ) =Aoy-MJCO (5) 

where MJCO equals 

=wzlwyl (rb/Glz ) -w a z(rb/Gly) -w*xl (rb/Gly ) 

+ wxlwyl(rb/Glx) 

and 

Azl + wdxl ( rb/Gly ) -wdy 1 ( rb/Glx ) =Aoz-MKC0 (6) 

HKCO equals 

=wxlwzl ( rb/Glx )Vxl (rb/Glz )-w?yl( rb/Glz) 

+ wylwzl( rb/Gly ) 

3. Sum, gf Horngat. .Equations 

Computing the sum o£ the moment equations about the 
center of gravity results in: 

£Hl=(rOAJl X FO ) + <rl/Gl X F1)-T1 + TO 
where the vector r O/G l is the distance from joint zero to 
the center of gravity of link one and vector rl/Gl is the 
distance from joint one to the center of gravity of link 
one, in the x, y and z directions. Such that 
rO/Gl = rjo-rGl 

and 

rl/Gl = rjl-rGl 

SO 

rjO-r£l=(xjO-xGl)i + (yjO-yGl)j + (zjO-zGl)k 

and 
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rjJ-rjGl = (xjl-xGl)i + (yjl-yGl)j + zjl-zGl)k 
In the x, y and z directions the sum of moment equations 
are: 

EMI in x directions 

( -yjO/Gl )FzO + (zjO/Gl)FyO + (yj 1/G1 )Fzl-( z j 1/G1 )Fyl 
-Tlx + TOx (7a) 

EMI in y directions 

( -z jO/Gl )FxO + (xjO/Gl )FzO + (zjl/Gl)Fxl- (xjl/Gl)Fzl 
-Tly + TOy (8a) 

EMI in z directions 

( -xjO/Gl)FyO + ( yjO/Gl )FxO + ( xj 1/G1 )Fyl-(y j 1/G1 )Fxl-Tlz 
+ TOz (9a) 

From Ref. 26, equation (517) pp.227 the sum of the moments 
about a fixed point that does not move with the body is 

equal to the time rate of change of angular momentum of the 

* » 
system (H) about the fixed point, EM=H. In the present 

study we have let each link be a composite body of two 

elements. The angular momentum (H) for a composite body 

where the number of elements of the body is two, about the 

center of gravity of each link is Hi=E* [Ri X (w X Ri)]Mi, 

<>» 

where Ri is the distance from the center of gravity of each 
link to the appropriate element (lor2) in the x, y and z 
direction. So: 

Hx = E* (Ryi(wx(Ryi)-wy(Rxi) ) -Rzi ( wz (Rxi ) wx-(Rzi ) ) ]Mi 

Uv 

Hx s(R t y i( WX )-Ryl(Rxl)(wy)-Rz(Rxl)(wz) + Rzl(wx)]Ml 

+ [R 4 y 2 ( wx ) -Ry2 ( Rx2 ) ( wy ) -Rz2 (Rx2 ) ( wz ) + (R*z2)wx]M2 
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If I xx = JRy 

and Ixy * r 

and 



If I xx 



Ry 2 +Rz^ dm. 



RxRy dm. 



and 



Ixz =fRxRy 



dm. 



then: 



Hx = (Ilxx(wx) - Ilxy(wy) - Ilxz(wz)]Ml 



+ II2xx(wx) - I2xy(wy ) - I2xz(wz)]M2. 



and 



HDx = [Ilxx(wdx) - Ilxy(wdy) - Ilxz(wdz)JMl 

+ (I2xx(wdx) - I2xy(wdy) - I2xz(wdz)]M2 (7b) 
by assuming the moment of inertia does not change with time 
but is constant for a given time interval. 

By similar analysis it can be shown: 



Hy= £ (Rzi(wy(Rzi)-wz(Ryi) )-Rxi(wx(Ryi)-wy(Rxi) ) ]Mi 



Hz=£ ( (Rxi(wz(Rxi)-wx(Rzi) ) -Ryi ( wy ( Rzi ) -wz(Ryi ) ) ]Mi. 




then: 



HDy = [ I lyy ( wdy ) -I iy z ( wdz ) -I iyx ( wdx ) ) M i 



+ CI2yy(wdy) -I2yz(wdz)-I2yx(wdx) JM2 



(8b) 



and 



If 




So 
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Hz=f Ilzz(wz)-Ilyz(wy)-Ilzx(wx) JM1 

+ I I2zz( wz) -I2yz( wy)-I2zx( wz) JM2 

then 

HDz=[Ilzz(wdz)-Ilyz(wdy )-Ilzx(wdx) ]M1 

+ [I2zz(wdz)-I2yz(wdy)-I2zx(wdx) ] M 2 (9b) 

Combining equations (7a) and (7b) and keeping known 
variables on the right side and unknown variables on the 
left side yields: 

EMlx= (-yjo/Gl)Fzo + (zjo/Gl)Fyo + (yjl/Gl)Fzl 

- (zjl/Gl)Fyl-HDx»Tlx-Tox (7) 

Combining equations (8a) and (8b) yields: 

£Mly=( -z jo/Gl ) Fxo + (xjo/Gl)Fzo + (zjl/Gl)Fxl 

- (x j 1/G1 ) Fzl-HDy=Tly-Toy (8) 

combining equations (9a) and (9b) yields: 

£Mlz=-(xjo/Gl)Fyo + (yjo/Gl)Fxo + (xjl/Gl)Fyl 

- ( y j 1/G1 ) Fxl-HDz=Tlz-Toz (9) 

D. LINK TWO EQUATIONS 

1. Sum of Forces Equations 

From the free body diagram (Figure 2) it follows 

that 

£Fx*Fx2 - Fxl=M2ax2 (10) 

£Fy=Fy2 - Fyl=M2ay2 (11) 

EFz=Fx2 - Fzl=M2az2 (12) 
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2. Joint Eq uations 



Analysis is conducted at joint one where similar 
equations are used as in joint zero with a point on link 
one (a) and one on link two (b). For point a the equation 
is 

Aa= A1+ wdl X ra/Gl + wl X (wl X ra/Gl) 

/N# SS* A/ 

ra/Gl is a vector whose distance is measured from point a 

A/ 

to the center of gravity of link one in the x, y and z 
direction. 

ra/Gl=( jxl-LCOGxl ) i + ( jyl-LCOGyl ) j + ( j zl-LCOGzl )k 
=ra/Glx + ra/Gly + ra/Glz 
For point b the equation is: 

Ab=A 2 + wd2 X rb/G2 + w2 X (w2 X rb/G2) 

a* /V /V /V ^ 

where rb/G2 is a vector whose distance is measured from 

/v 

point b to the center of gravity of link two. 

rb/G2=( jxl-LC0Gx2)i + ( jyl-LC0Gy2 ) j + ( j zl-LC0Gz2 )k 
=rb/G2x + rb/G2y + rb/G2z 

Equating Aa and Ab and setting knowns and unknowns on 
the respective sides of the equation results in: 

Ax2-Axl + wdy2 (rb/G2z ) -wdz2 (rb/G2y ) -wdyl (ra/Glz ) + 
wdzl ( ra/Gly ) =MIC1-MIC2 (13) 

MICl“wylwxl (ra/Gly )-w2yl (ra/Glx )-w2zl (ra/G lx ) 

+ wzlwxl ( ra/Glz ) 

MIC2=wy2wx2 (ra/G2y )-w2y2(ra/G2x)-w2z2(rb/G2x) 

+ wz2wx2(rb/G2z) 
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Ay2-Ayl + wdz2 (rb/G2x ) -wdx2( rb/G2z ) -wdzl ( ra/Glz ) 

+ wdxl ( ra/Glz ) =MJC1-HJC2 (14) 

MJCl=wzlwyl (ra/Glz) -w2zl(ra/Gly) -w2xl (ra/Gly ) 

+ wxlwyl ( ra/Glx) 

HJC2=wz2wy2(rb/G2z)-w2z2(rb/G2y )-w2x2(rb/G2y ) 

+ wx2wy2( rb/G2x) 

AZ2-AZ1 + wdx2(rb/G2y)-wdy2(rb/G2x)-wdxl(ra/Gly) 

+ wdyl(ra/Glx)= MKC1-MKC2 (15) 

HKCl=wxlwzl( ra/Glx )-w2xl( ra/Glz )-w2yl( ra/Glz > 

+ wylwzl( ra/Gly ) 

MKC2=wx2wz2( rb/G2x) -w2x2( rb/G2z) -w2y2(rb/G2z ) 

+ wy2wz2( rb/G2y ) 

3. Saw of the HQirent Eqs^Uopg 

These equations have a similar development as that 
of link one: 



ZH2 =(r j 1/G2) X FI + (rj2/G2) X F2 + T1-T2 
where 

r j 1/G2=( xj l-xG2) 1 + (yjl-yG2)j + (zjl-zG2)k 
r j 2/G2 = ( x j 2-xG2 ) 1 + (yj2-yG2)j + (zj2-zG2)k 
ZH2x= -(yj l-yG2 )Fzl + (zjl-zG2)Fyl + (yj2-yG2)Fz2 

- ( z j 2-zG2 ) Fy2 + Tlx-T2x 

ZH2y= -(zjl-zG2)Fxl + (xjl-xG2)Fzl + (zj2-zG2)Fx2 

- ( x j 2-xG2 ) Fz2 + Tly-T2y 

ZM2z=- (xj l-xG2)Fyl + (yjl-yG2)Fxl + (xj2-xG2)Fy2 

- (yj2-yG2)Fx2 + Tlz-T2z 



(16a) 



(17a) 



(18a) 



From angular momentum equation developed for link one, it 
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can be shown for link two: 



£M2x=HDx 


(16b) 


£M2y=HDy 


(17b) 


EM2z— HDz 


(18b) 



Combining equations (16a) and (16b) the following result: 
-(yjl-yG2)Fzl + ( z j l-zG2 )Fyl + (y j 2-yG2 )Fz2-( z j 2-zG2 )Fy2 
-HDx=-Tlx + T2x (16) 

Combining equations (17a) and (17b) yield the following 
result: 

-(zjl-zG2)Fxl ♦ (xj l-xG2 )Fzl + ( z j 2-zG2 )Fx2-( x}2-xG2 )Fz2 
-HDy=-Tly + T2y (17) 

Combining equations (18a) and (18b) yield the following 
result: 

-(xjl-xG2)Fyl + (yjl-yG2)Fxl + (xj2-xG2)Fy2-(yj2-yG2)Fx2 
-HDz=-Tlz + T2z (18) 

E. LINK THREE EQUATIONS 

1. Sum of Forces Equations 

Following similar logic from that previously 
developed: 

EFx- -Fx2=H3ax3 (19) 

£Fy» -Fy2=H3ay3 (20) 

EFz= -Fz 2 - H3=M3az3 (21) 

2. Joint Equations 

Kith point a on link two and point b on link three 
one gets for joint equations at joint two: 
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Aa=A2 + ( wd2 X ra/G2) + w2 X (w2 X ra/G2) 

/V ''V Ay •’V ^ /Ay 

where ra/G2 is a vector whose distance is measured £rom 
point a to center of gravity of link two in the x,y and z 
direction. 

ra/G2=( jx2-LCOGx2)i + ( jy2-LCOGy2 ) j + ( j z2-LCOGz2 )k 
=ra/G2x t ra/G2y + ra/G2z 
For point b 

Ab=A3 -i- wd3 X rb/G3 + w3 X (w3 X rb/G3) 

/v /a. /<y ^ 

where rb/G3 is a vector whose distance is measured from 

Ay 

point b to center of gravity of link three in the x, y and 
z direction. 

rb/G3=( jx2-LCOGx3 ) i + ( jy2-LCOGy3 ) j + ( j z2-LCOGz3 )k 
=rb/G3x + rb/G3y + rb/G3z 

Equating Aa and Ab and setting knowns and unknowns on the 
respective sides of the equation results in: 

Ax3-Ax2 + wdy3(rb/G3z)-wdz3(rb/G3y)-wdy2(ra/G2z) 

+ wdz2 ( ra/G2y)=MIC3-MICl (22) 

MIC3=wy2wx2(ra/G2y ) -w2y2 (ra/G2x)-w2z2 (ra/G2x) 

-i- wz2wx2(ra/G2z) 

MIC4= wy3wx3(rb/G3y )-w2y3(rb/G3x)-w2z3(rb/G3x) 

+ wz3wx3(rb/G3z) 

Ay 3 -Ay2 ♦ wdz3(rb/G3x)-wdx3(rb/G3z )-wdz2(ra/G2x) 

+wdx2(ra/G2z) =MJC3-MJC4 (23) 

HJC3=wz2wy2(ra/G2z ) -w2z2(ra/G2y ) -w2x2(ra/G2y ) 

+ wx2wy2(ra/G2x) 
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MJC4=wz3wy3(rb/G3z) -w2z3(rb/G3y ) -w2x3 ( rb/G3y ) 

+ wx3wy3(rb/G3x) 

AZ3-AZ2 + wdx3( rb/G3y ) -wdy3 ( rb/G3x ) -wdx2 ( ra/G2y ) 

+ wdy2(ra/G2x)= MKC3-MKC4 (24) 

MKC3=wx2wz2(ra/G2x)-w2x2(ra/G2z)-w2y2(ra/G2z) 

+ wy2wy2(ra/G2y) 

MKC4=wx3wz3(rb/G3x)-w2x3(rb/G3z ) -w2y 3 ( rb/G3z ) 

♦ wy3wz3 ( rb/G3y ) 

3» Soro of Moment Equations 

As in the development of the equations for link one: 
SM3=( r j 2/G3 ) X F2 + T2 

/v ^ /V /V" 

where r j 2/G3=( xj 2-xG3) i + (yj2-yG3)j + (zj2-zG3)k 



=xj 2/G3 + yj 2/G3 + zj2/G3 

EM3x=(-yj2/G3)Fz2 + (zj2/G3)Fy2 + T2x (25a) 

EM3y= ( -z j 2/G3 ) Fx2 + (xj2/G3)Fz2 + T2y (26a) 

EM3z=( -xj 2/G3 >Fy2 + (yj2/G3)Fx2 + T2z (27a) 

From the angular momentum theory: 

EM3x=HDx (25b) 

EM3y»H0y (26b) 

EM3z=HDz (27b) 

Combining equations (25a) and (25b) the following results 
( -yj2/G3 )Fz2 + ( zj 2/G3 )Fy2-HDx= -T2x (25) 

Combining equations (26a) and (26b) the following results 
( ( -z j 2/G3 )Fx2 +( xj 2/G3 )Fz2-HDy= -T2y (26) 

Combining equations (27a) and (27b) the following results: 
( -xj 2/G3 )Fy2 + (y j 2/G3 )Fx2-HDz= -T2z (27) 
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IV. COMPUTATIONAL APPROACH 



The language chosen to write the program was the 
Digital Simulation Language (DSL) using Fortran 77 coding. 
This language does an excellent dynamic simulation that 
allows the user to be interactive, with real time 
processing vice batch mode processing commonly used with 
the Continuous System Modelling Program (CSMP), and all 
calculations done in double precision. The source code 
produced for this program was complied on an IBM 3033 
computer using a Fortran 77 compiler. 

A. PRINCIPLE PROGRAM MATRICIES 

A 27x27 Matrix A (MatA) was created from the 
coefficients of the unknowns (forces, linear acceleration 
and angular acceleration) from equations (1) to (27). 
Correspondingly a 27x1 Matrix B (MatB) was generated from 
equations (1) to (27) from all knowns (torques, angular 
velocities, link masses, and various positions). Subroutine 
CPROD was used to conduct all cross products required in 
the main program. Subroutine (LEQT2F) was then called from 
the IMSL library. This subroutine takes MatA inverts it, 
multiplies it by MatB and solves the generalized equation 
Ag=b for the b vector using Gaussian elimination with 
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iterative improvement to get accuracy within six decimal 
digits. The output from LEQT2F returns from the subroutine 
via MatB. This output is then used by the IbITGRL DSL 
statement to take the integral of angular acceleration 
(wdx, wdy, wdz) to get angular velocity (wx, wy, wz) and 
again to get the position of the link with respect to theta 
(c6x, cdy, c0z) for each torque input per time step. The 
cartesian orientations are converted to Euler angles (0x, 
8y, 9z) prior to returning to the beginning of the program. 
The method used to solve the second order differential, 
equation for accelerations is invoked by ADAMS which is the 
second order, variable step integration ADAMS method. This 
method was shown to be the fastest (CPU time) and most 
accurate of the methods available [Ref. 8). Similarly, 
INTGRL is applied to find the linear acceleration (A) of 
each link, velocity (V,) and finally the position of the 
center of gravity of the link. These newly found values are 
fed back into the beginning of the simulation program for 
the next time step until the end of the interval. This 
process is summarized in Figure 3. 
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Figure 3. Computer Simulation Flock Diagram 
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Constraints are also built into the simulation program 
that enable the operator to limit the movement of the links 
in the yz plane for a two dimensional demonstration of 
link-three-only movement. The constraints are also used for 
link two and three movement. The constraints are applied by 
zeroing out a row, except for the diagonal which is set to 
1.0. The MatB entry is set to the constrained value. 

It is during this simulation that a differentiation 
should be made between the cartesian theta (c6) position 
developed by the INTGRL function (Figure 4a) and the Euler 
angler theta (0) used as direction angles in computing 
distances (Figure 4b). When in the yz plane the angular 
acceleration is about the x axis and when the integral is 
taken twice with respect to time, what results is the angle 
theta about the x axis. This cartesian angle is defined as 
cOx and is obviously not the same as the theta angle used 
to position the link initially which is defined as 6y. To 
resolve this discrepancy when cOx is computed it is 
converted to the euler angle Oy by setting the two equal so 
6y=c9x, in a two-dimensional simulation. Additionally, 
euler angle 0z=90°-9y and 0x=9O° whenever simulating, two- 
dimensional yz plane motions. 
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Figure 4. Angles of Orientation (a) Cartesian angles (b) Euler angles. 
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B. PROGRAM VALIDATION 



Validation of the simulation program takes place in two 
ways . 

l. Vft iUfla Jtl p n ol On e. Ll nK Csse 

For link three the theoretical value of theta in the 
z direction (thdx3), is compared to the value of theta in 
the x direction (0x3) that the simulation computed for each 
time step (Appendix A has the program listing). 

As a test case, the torque delivered at joint two was 
assumed to be: 

T2x=10sin(2xt) . 

Also, Ixxa, the moment of inertia about joint two, is equal 
to mass at the end of the link M(3,2) times the distance 
from joint two to the mass at the end of the link, squared 
or. 



Ixxa=M( 3,2) x (L(3,2) + L(3,l)) . 

This may be used to solve for thdx by taking the integral 
with respect to the time which results in: 



th8x3= T2x 

Ixxa 




t th0x3 dt= -10 cos (2*t) 



Ixxa( 2x ) 



o 



th0x3 c ( -10 



(cos2xt) + 10 ) 1 

2 % Ixxa 



2x 



* . 



'o 



thdx3 dt 



th0x3 dt= th8x3=( -10 

4x**2 



sin(2xt) + lOt )1 + i 

2x Ixxa 4 
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For comparison % error is used so 

% error = ( th03-0x31 x 100 
max thdx3 

2. Validati on of Two Links Case 

For the validation of two links the computed torques 
at joints two and joint one (Tory2x, Torylx) are compared 
to the torques that are actually input (T2x, Tlx) at each 
time step (Appendix B has the program listing). If there 
are no effects of singularity then the theoretical torque 
and input torque should be very similar. From Figure 2 the 
sum of the moments about the center of gravity of link 
three is: 

EM3=M3(L(3,2)2( wdx( 3 ) ) =T2x + Fz2y - Fy2z 
so 

T2x=M3 ( L( 3, 2 ) ) 2( wdx( 3 ) ) - Fz2y + Fy2z=Tory2x 
where 

y=L( 3, 1 ) (cos(0y3 ) ) 

Fy2=( -M3 ) ( ay3 ) 
z=L( 3, 1 ) ( cos ( 0y 3 ) ) 

Fz2=( -M3 ) ( az3 ) 

Sum of the moments about the center of gravity of link two 
is : 

EM2=Ixx2 ( wdx ( 2 ) ) =Tlx-T2x + Fzlcos (0y2 ) ( L( 2, 1 ) ) 

-Fy lsin(0y2 ) ( L( 2, 1 ) ) + Fz2cos(6y2 ) ( L( 2, 2 ) ) 

-Fy2sin(0y2) (L( 2, 2) ) 
so 
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Tlx=M2 ( L( 2,2) ) 2 ( wdx ( 2 ) ) + T2x-Fzlcos (0y2 ) ( L( 2, 1 ) ) 

+ Fylsin(8y2)(L(2,l) )-Fz2(L(2,2) )cos(0y2) 

+ Fy2sin(0y2) (L( 2, 2) )=Torylx 
where 

Fzl=Fz2-M2(az2 ) 

Fy l=Fy2-M2ay2 . 

For comparison the % error is used for difference in 
torque for joint two and joint one: 

Errt2x=(Tory2x-T2x)/(maxTory2x) x 100 
Errtlx=( Torylx-Tlx) / (maxTorylx) x 100 
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V. RESULTS 



A. MOVEMENT OF LINK THREE 

Analysis of the movement of only link three shows very 
good results for program validation. Figure 5 shows a plot 
of Euler angles for both theoretical (thdy3) and simulated 
(6y3) values, the graph shows indistinguishable 
differences. To further visualize the difference Figure 6 
was plotted, which is the % error between thdy and dy 
versus time. There seems to be greater error (0.0032%) at 
around 0.8 seconds than at 0.2 seconds. This could possibly 
be caused by error buildup in the computation due to round 
off error from subroutine LEQT2F and truncation error from 
approximating the solution to the second order differential 
equation by the ADAMS method. Additionally, inaccuracies 
could occur in estimating the value of x and using it in 
trigonometric calculations . However, the % error is small and 
is acceptable to verify the proper operation of the program 
for the single degree of freedom case. 
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0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 

TIME (SEC) 

Figure 6. Percent Error Between Theoretical and Simulated 
Fuler Angle vs. Time 



B. MOVEMENT OF LINK TWO AND THREE 



Analysis o£ the movement of link two and three is the 
crucial test of how the simulation deals with the problem 
of singularity. A torque was input to joint two and an 
opposite torque to joint one. At some point the alignments 



of the 


two 


links will 


have 


some 


absolute angle of 


0o 


(Figure 


7) 


relative to 


each 


other . 


At 


this time 


if 


singularity 


exists there 


is no 


longer 


any 


control of 


the 



links and accelerations and velocities vary abnormally, 
never returning to the level they were at before the 
singular position was reached [Ref. 8]. So the reason for 
comparing the values of the computed torques (Tory2x, 
Torylx) given the position variables solved for by the 
simulation program and the torques input to the joints 
(T2x, Tlx), is to check for abnormalities. Figure 8 shows 
the graph of computed and input torques for joint two and 
Figure 9 shows the graph of computed and input torques for 
joint one versus time. The two curves match very well and 
shows almost no deviation between them for the scale used. 

When the % errors are plotted between computed torque 
and input torque versus time for links two and one (Figures 
10 and 11) again very little % error is observed with the 
largest being around 0.024% at time 2.8 seconds for torque 
input at joint one. This may be attributed to the similar 
reasons as the one-link since now both link two and three 
are moving these errors are building as time increases. It 
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Figure 7. Simulated Movement of l.ink Two and Three vs. Time 
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Figure 8. Computed and Input Values of Torques 
at Joint Two vs. Time 
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Figure 9. Computed and Input Values of Torques 
at Joint One vs. Time 
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ri m i*: (s kc) 



is also 


observed in Figure 


10 


that 


the 


% 


error 


is not 


smooth 


but 


erratic and 


causes a 


"spikey " 


curve fit. 


However, 


the 


error comes 


back 


down 


to 


the 


zero 


plateau 



instead of remaining at a high level which is what would 
have happened had singularity occurred. The overall % 
errors are small and so lends credability to the simulation 
model. Figure 7 was plotted to see at what point the two 
links align themselves and to get a picture of about how 
long they are close (within one degree) to the point of 
singularity. It appears to be 0.5 seconds which is enough 
time for singularity to have a strong effect [Ref. 8). 
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VI. CONCLUSIONS 



The ability of a global two degree of freedom robot arm 
to maneuver through a point of singularity under applied 
torques was demonstrated. This was verified by comparing 
the computed torque at joint one and two in the x direction 
to the values that were input. There were no unusual or 
abnormal results occurring in the acceleration or 
velocities and so little error was produced. 
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VII. RECQHHENPftTIQNS 



The following recommendations are provided: 

1. Develop a linearized manipulator model and a 
corresponding controller for the two degree of freedom 
case . 

2. Validate the approach via actual empirical tests for the 
two dimensional case. This will establish the difficulty of 
determining accurate constants for the simulation and 
controller design. 

3. Demonstrate the model and controller in 3 dimensions 
with 3 links. The difficulty here arises in analyzing the 
direction of a given joint torque in 3 dimensions. This 
could probably be done by finding a unit normal vector 
perpendicular to the joint in the x, y and z direction and 
multiplying it by the torque magnitude. 

4. Validate the approach by implementation for the three 
dimensional case. 
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APPENDIX A 

SIMULATION PROGRAM FOR MOVEMENT OF LINK THREE 



TERMINAL 
METHOD ADAMS 

PRINT .01 ,ETHETY(3) ,EULORY(3) ,ERROR(3) 

CONTROL FINTIM =1.0, DELMAX =.01, DELPRT = .01 
SAVE .01, ETHETY ( 3 ) , EULORY ( 3 ) ,ERROR(3) 

GRAPH ( DE=TEK6 1 8 ) TIME , ETHETY(3) 

GRAPH ( DE=TEK6 1 8 ) TIME ,ERROR(3) 

GRAPH(DE=TEK618) TIME ,EULORY(3) 

D DIMENSION MATA (27 , 27 ) ,MASS (3,2) ,L(3.2) ,RX(3,2) ,RY(3 , 2) , RZ(3 , 2) 

D DIMENSION IXX(3 , 2 ) , IXZ (3 , 2 ) , IXY(3 , 2 ) , IYY(3 ,2 ) , IYZ(3 , 2 ) , IZZ(3 , 2 ) 

D INTEGER IER , I , RUN ,M ,N , I A , IDGT 
EXCLUDE IA , IDGT , IER , I , RUN ,M ,N 

ARRAY MATB(27 ) ,LCOGX(3) ,LCOGY(3) , LCOGZ ( 3 ) , ETHETX ( 3 ) , ETHETY ( 3 ) . ETHETZ ( 3 ) 
ARRAY CTHETX(3) , CTHETY(3) , CTHETZ(3) ,THDDOT(3) , IXXA(3 ) , ERROR (3) 

ARRAY VECTA0 (3 ) , VECTB0 ( 3 ) , VECTA1 ( 3 ) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 

ARRAY WDX(3 j .WDY(3) ,WDZ(3) ,WX(3) ,WY(3),WZ(3) ,RBG1(3) ,RAG1(3) ,THEORY(3) 
ARRAY RBG2(3) ,RAG2(3) ,RBG3(3) ,THETXR(3) , THETYR(3) , THETZR(3) ,EULORY(3) 
ARRAY HDX ( 2 ) , HDY ( 2 ) , HDZ ( 2 ) 

ARRAY SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,WKAREA(850) 

D DATA MATA/729 * 0./ 

INITIAL 

* INPUT PARAMETER CONSTANTS 
A = 10.0 
P = 0.0 
W = 2*PI 
IDGT = 4 
G=0 . 0 
N=27 
M=1 

IA =27 
RUN = 1 



* INPUT JOINT LOCATIONS IN METERS 

JXO = 0.0 
JYO = 0.0 
JZO = 0.0 
JX1 = 0.0 
JY1 = 1.0 
JZ1 = 0.0 
JX2 = 0.0 
JY2 = 2.0 
JZ2 = 0.0 

* INPUT TORQUE CONSTANTS 

TOX =0.0 
TOY = 0.0 
TOZ = 0.0 
T1X = 0.0 
T1Y = 0.0 
T1Z = 0.0 
T2Y =0.0 
T2Z = 0.0 

*INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS FOR EACH LINK ENDS 



1,1) 


= 0.50 


1,2 


= 0.50 


2,1 


= 0.50 


2,2 


= 0.50 


3,1) 


= 0.50 
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L(3,2) = 0.50 

* INPUT MASS AT LINK ENDS IN KILOGRAMS 



MASS < 


1 , 1 ) 


= 


2.5 


MASS < 


1 - 2 } 


= 


2.5 


MASS \ 


2 , 1 ) 


= 


2.5 


MASS < 


2,2) 


= 


2.5 


MASS i 


3,1) 


= 


2.5 


MASS \ 


3,2 


S 


2.5 



INPUT OMEGA AND OMEGA DOT 
DO 30 I = 1,3 



WX(I) 


= 0.0 


WY(I) 


= 0.0 


WZ(I) 


= 0.0 


WDX( I ] 


) = 0.0 


WDY ( I 


) = 0.0 


WDZ(i; 


) = 0.0 



30 CONTINUE 

* INPUT INITIAL VALUES OF EULER ANGLE THETA AND CONVERT TO RADIANS 

ETHETX(l) = 90.0 
TX1 = ETHETX(l) * DEGRA 
ETHETY ( 1 ) = 0.0 
TY1 = ETHETY(l) * DEGRA 
ETHETZ(l) = 90.0 
TZ1 = ETHETZ(l) * DEGRA 
ETHETX(2) = 90.0 
TX2 = ETHETX(2) * DEGRA 
ETHETY (2 ) = 0.0 
TY2= ETHETY(2) * DEGRA 
ETHETZ(2) = 90.0 
TZ2 = ETHETZ(2) * DEGRA 
ETHETX(3 ) =90.0 
TX3 = ETHETX(3) * DEGRA 
ETHETY (3 ) = 45.0 
TY3 = ETHETY(3) * DEGRA 
ETHETZ(3) = 45.0 
TZ3 = ETHETZ(3) * DEGRA 

* INPUT LOCATION OF LINK CENTERS OF GRAVITY 

LCOGX(l) =0.0 
XI = LCOGX(l) 

LCOGY(l) =0.5 
Y1 = LCOGY(l) 

LCOGZ(l) =0.0 
Z1 = LCOGZ(l) 

LCOGX(2) = 0.0 
X2 = LCOGX(2) 

LCOGY ( 2 ) =1.5 
Y2 = LCOGY (2) 

LCOGZ(2) =0.0 
Z2 = LCOGZ(2 ) 

LCOGX(3 ) =0.0 
X3 = LCOGX(3) 

THERAD = ETHETY(3 ) * DEGRA 

LCOGY ( 3 ) = 2.0 + COS (THERAD) * L(3,l) 

Y3 = LCOGY(3) 

LCOGZ(3) = L(3, 1) * SIN(THERAD) 

Z3 = LCOGZ(3) 

* INPUT MASS OF EACH LINK IN KG AND COMPUTE WEIGHTS IN NEWTONS 

MAS SI =5.0 
MASS 2 =5.0 
MAS S3 =5.0 
W1 = MASSING 
W2 = MASS2*G 
W3 = MASS3*G 
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INPUT ACCELERATION OF JOINT ZERO 
AOX = 0.0 
AOY = 0.0 
AOZ = 0.0 



DERIVATIVE 

NOSORT 

* INPUT JOINT EQUATIONS 

* INITIALIZE MATRIX B TO ZERO 

DO 10 I = 1,27 
MATB(I) =0.0 
10 CONTINUE 

* INPUT TORQUE AT JOINTS 

T2X = A*SIN (W*TIME +P) 

* JOINT ZERO AB = AG1 + (WD1 X RB/G1) + W1 X (W1 X RB/G1) 

VECTAO ( 1 ) = WDX(l) 

VECTA0(2) = WDY(l) 

VECTAO (3) = WDZ(l) 

RBG1(1) = JXO - LCOGX(l) 

RBG1 ( 2 ) = JYO - LCOGY(l) 

RBG1 ( 3 ) = JZO - LCOGZ(l) 

CALL CPROD ( VE CTAO , RBG1 , MI AO , MJAO , MKAO ) 

VECTAO ( 1 ) = WX(1) 

VECTAO (2) = WY( 1 ) 

VECTAO (3) = WZ(1) 

CALL CPROD (VECTAO , RBG1 , MIBO , MJBO , MKBO ) 

VECTBO(l) = MIBO 
VECTBO (2) = MJBO 
VECTBO ( 3 ) = MKBO 

CALL CPROD(VECTAO , VECTBO ,MICO ,MJCO ,MKCO) 

* JOINT ONE EQUATIONS--- AA = AG1 + (WD1 X RA/G1) + W1 X (W1 X RA/G1 ) 

VECTAl(l) = WDX(l) 

VECTA1 ( 2 ) = WDY(l) 

VECTA1(3) = WDZ(l) 

RAG1(1) = JX1 - LCOGX(l) 

RAG1 ( 2 ) = JY1 - LCOGY(l) 

RAG1 (3 ) = JZ1 - LCOGZ(l) 

CALL CPROD ( VECTA1 ,RAG1 ,MIA1 ,MJA1 ,MKA1 ) 

VECTA1 ( 1 ) = WX(1) 

VECTA1 ( 2 ) = WY(1) 

VECTA1 ( 3 ) = WZ(1) 

CALL CPROD (VECTA1 ,RAG1 ,MIB1 ,MJB1 ,MKB1 ) 

VECTB1 ( 1 ) = MIB1 
VECTB1 l 2) = MJB1 
VECTB1(3) = MKB1 

CALL CPROD (VECTA1 , VECTB1 ,MIC1 ,MJC1 ,MKC1 ) 

* AB = AG2 + (WD2 X RB/G2) + W2 X (W2 X RB/G2 ) 

VECTA1 ( 1 ) = WDX(2) 

VECTA1 (2 ) = WDY ( 2 ) 

VECTA1 (3) = WDZ ( 2 ) 

RBG2(1) = JX1 - LC0GX(2) 

RBG2(2) = JY1 - LCOGY (2) 

RBG2 ( 3 ) = JZ1 - LC0GZ(2) 

CALL CPROD (VECTA1 , RBG2 ,MIA2 ,MJA2 ,MKA2 ) 

VECTA1 ( 1 ) = WX(2) 

VECTA1 ( 2 ) = WY(2) 

VECTA1 (3) = WZ( 2) 

CALL CPROD (VECTA1 , RBG2 ,MIB2 , MJB2 ,MKB2) 

VECTB1 ( 1 ) = MIB2 
VECTB1 ( 2 ) = MJB2 
VECTB1(3) = MKB2 

CALL CPROD (VECTA1 , VECTB1 ,MIC2 ,MJC2 ,MKC2 ) 
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* * * * * 



JOINT TWO EQUATIONS 

AA = AG2 + (WD2 X RA/G2) + W2 X (W2 X RA/G2) 
VECTA2 ( 1 ) = WDX(2) 

VECTA2(2) = WDY(2) 

VECTA2 (3 ) = WDZ(2) 

RAG2 ( 1 ) = JX2 - LCOGX(2 ) 

RAG2(2) = JY2 - LCOGY(2) 

RAG2 (3) = JZ2 - LCOGZ(2) 

CALL CPROD (VECTA2 , RAG2 ,MIA3 ,MJA3 ,MKA3 ) 
VECTA2 ( 1 ) = WX(2 ) 

VECTA2 ( 2 ) = WY(2) 

VECTA2(3) = WZ(2) 

CALL CPROD (VECTA2 ,RAG2 ,MIB3 ,MJB3 ,MKB3 ) 
VECTB2 ( 1 ) = MIB3 
VECTB2 ( 2 ) = MJB3 
VECTB2 (3) = MKB3 

CALL CPROD (VECTA2 , VECTB2 ,MIC3 ,MJC3 , MKC3 ) 

AB = AG 3 + (WD3 X RB/G3 ) + W3 X (W3 X RB/G3 ) 
VECTA2 ( 1 ) = WDX(3) 

VECTA2 ( 2 ) = WDY ( 3 ) 

VECTA2(3) = WDZ (3) 

RBG3 ( 1 ) = JX2 - LCOGX(3) 

RBG3(2) = JY2 - LCOGY(3) 

RBG3 ( 3 ) = JZ2 - LCOGZ(3) 

CALL CPROD ( VECTA2 , RBG3 , MI A4 , MKA4 , MKA4 ) 
VECTA2 (1 ) = WX(3 ) 

VECTA2(2) = WY{3) 

VECTA2 (3) = WZ(3) 

CALL CPROD (VECTA2 , RBG3 ,MIB4 ,MJB4 , MKB4) 
VECTB2 ( 1 ) = MIB4 
VECTB2(2) = MJB4 
VECTB2 ( 3 ) = MKB4 

CALL CPROD (VECTA2 , VECTB2 ,MIC4,MJC4,MKC4) 



SUM OF MOMENTS EQUATIONS 

CONVERT EULER ANGLES FROM DEGREES TO RADIANS 
DO 40 I = 1,3 

THETXR ( I ) = ETHETX(I) * DEGRA 

THETYR(I) = ETHETY(I) * DEGRA 

THETZR(I) = ETHETZ(I) * DEGRA 



* 



COMPUTE HX DOT ,HY DOT, 
RX(I , 1 ' 

RX( I , 2 ) = L 
RY 1,1 
RY ( 1 , 2 ) = L 
RZ(1 ,1 
RZ( I , 2 ) = L 
IXX(I , 1 )=MASS (1,1 
IXX(I,2 =MASS(I,2 
IXZ( I , 1 )=MASS (1,1 
IXZ(I ,2 )=MASS( I , 2 
IXY( I , 1 )=MASS (1,1 
IXY(I ,2)=MASS(I,2 
HDX(l) = WDX 
HDX(2 ) = WDX 
IYY(I , 1 ) = MASS ( I 
IYY(I ,2) = MASS ( I 
IYZ( I , 1 ) = MASS ( I 
IYZ(I ,2) = MASS ( I 
HDY(l) = WDY 
HDY(2) = WDY 
IZZ(I,1) = MASS ( I 
IZZ(I ,2) = MASS ( I 



HDZ(l) = WDZ 
HDZ(2) = WDZ 
IXXA( I )=MASS (1,2) 
SUMHDX(I) = HDX( 1 



HZ DOT 

L ( 1 , 1 ) * COS (THETXR (I 
(1,2) * COS (THETXR( I ) 

L(1 , 1 ) * COS (THETYR(I 
(1,2) * COS (THETYR( I ) 

L( 1 , 1 ) * COS (THETZR( I 
(1,2) * COS (THETZR( I ) 
'*((RY(I,1)*RY(I,1))+(RZ(I, 
*((RY 1,2 *RY(I 2))t 

* RZ ( 1 , 1 ) * RX (1,1 

* RZ ( I , 2 ) * RX ( I , 2 

* RX ( 1 , 1 ) * RY ( 1 , 1 

* RX ( 1 , 2 ) * RY ( 1 , 2 
*IXX(I,1)-WDZ(I)*IXZ(I,1 
*IXX(I,2)-WDZ(I)*IXZ(I,2 

* ( (RX( I , 1 ) *RX( I , 1 ) ) + 

* ( ( RX ( I , 2 ) *RX (1,2)) + 

* RY ( I , 1 ) * RZ 1,1 

* RY ( I , 2 ) * RZ ( I , 2 ) 

*IYY (1,1) -WDX( I ) *IXY( I , 1 
*IYY( I , 2 )-WDX( I ) *IXY( I , 2 

* ( ( RX (1,1) *RX (1,1)) + 

* ( ( RX ( I , 2 ) *RX ( 1,2)5 + 
*IZZ( I , 1 ) -WDX( I )*IXZ(l , 1 

(l)*IZZ(I , 2) -WDX( I )*IXZ( I , 2 
^((L(I,2)+L(I,1 ) )**2) 

) + HDX(2) 






(RZ(l,2)*RZ( 



1' 

2 

,1 

,2 

’ l \ 

,1 

. 2 ) 



Irz(i;i)*rz(i , i))) 

(RZ(I,2)*RZ(I,2))) 



-WDY ( I ) *IXY (1,1 
-WDY(I )*IXY (1,2 



) -WDZ( I )*IYZ( I , 1 ] 

) -WDZ (I )*IYZ( I , 2 ! 

( RY ( 1 , 1 ) *RY ( I / 1 ) ) ) 
(RY(I .2)*RY(I,2))) 

)-wdy(i)*iyz(i,i; 

) -WDY ( I )*IYZ(I , 2 , 






* * * * * U> * * * 



40 

* 



SUMHDY ( 


;n 


= HDY ( 1 


) + HDY ( 2 ) 


SUMHDZ ( 


!i) 


= HDZ ( 1 


) + HDZ(2) 



CONTINUE 



TEST TO SEE WHICH CONSTRAINT IS IN EFFECT 1,2 OR 3 
IF (RUN .EQ. 1) GO TO 1 

IF (RUN .EQ. 2) GO TO 2 

IF (RUN .EQ. 3) GO TO 3 

* INITIAIIZE MATRIX ACCORDING TO CONSTRAINT 

1 DO 60 I = 1,18 

MATA ( I , I ) = 1.0 
60 CONTINUE 

GO TO 4 

2 DO 70 I = 1,9 

MATA ( I , I ) =1.0 
70 CONTINUE 

GO TO 7 

ENTER CONSTANTS INTO MATRIX A 
LINK ONE 

SUM OF FORCES IN THE X DIRECTION 
MATA ( 1 , 1 ) =1.0 

MATA( 1,4) = MAS SI 
MATA(l,10) = -1.0 

SUM OF FORCES IN Y DIRECTION 
MATA (2 , 2) = 1.0 
MATA (2 , 5) = MASS1 
MATA (2 , 11 ) = -1.0 

SUM OF FORCES IN Z DIRECTION 
MATA(3 , 3) =1.0 
MATA(3 , 6) = MASS1 
MATA (3 , 12 ) = -1.0 

SUM OF FORCES LINK ONE EQUAL 
MATB ( 3 ) = -W1 

EQUATIONS AT JOINT ZERO 
IN THE X DIRECTION 
MATA (4 , 4 ) =1.0 
MATA (4 ,8) = RBG1 (3) 

MATA (4, 9) = -RBG1 (2) 

MATB(4)= AOX - MICO 

IN THE Y DIRECTION 
MATA ( 5 , 5 ) =1.0 
MATA( 5,7) = -RBG1 (3) 

MATA( 5 , 9) = RBG1(1) 

MATB(5) = AOY - MJCO 

IN THE Z DIRECTION 
MATA (6, 6) = 1.0 
MATA(6,7) = RBG1(2) 

MATA (6, 8) = -RBG1(1) 

MATB(6) = AOZ - MKCO 

* SUM OF MOMENTS EQUATIONS FOR LINK ONE IN THE X,Y,Z DIRECTIONS 

MATA (7 , 2 ) = RBG1(3) 

MATA (7 , 3) = -RBG1 (2) 

MATA (7 , 7 ) = -(IXX(l.l) + IXX(1.2)) 

MATA (7,8) = IXY (1,1) + IXY(1,2) 

MATA (7 ,9) = IXZ(1,1) + IXZ(1,2) 

MATA(7 , 11 ) = -RAG1 (3) 
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* * * * X -J** 



MATA (7 , 12 ) = RAG1 (2) 



MATB(7) = T1X - TOX 

MATA (8,1) = -RBG1 (3) 
MATA(8,3 = RBGl(l) 
MATA(8,7 = IXY (1,1) 
MATA (8, 8) = - ( IYY ( 1 
MATAI8 , 9 ) = IYZ (1,1 
MATAI8 , 10 ) = RAG1 (3 

V = 



MATA(8,12; 



+ 

1 ) 

+ 



IXY (1,2) 

+ IYY(1 2)) 
IYZ( 1,2) 



■ RAG 1(1) 



MATB ( 8 ) = T1Y - TOY 



MATAI 

MATAI 

MATAI 

MATAI 

MATAI 

MATAI 

MATAI 



9.1 

9.2 

9.7 

9.8 

9.9 

9.10 



= RBG1 (2 ) 

= -RBG1(1) 

= IXZ(1,1) + 
= IYZ (1,1) + 
= -(IZZ(1,1) 



) = -RAG1 (2) 
9,11) = RAG 1(1) 



IXZ( 1 ,2) 
IYZ( 1,2) 

+ IZZ( 1,2)) 



MATB (9) = T1Z - TOZ 
LINK TWO 

SUM OF FORCES IN X DIRECTION 
MATA( 10,10) = 1.0 

MATA(10 , 13) = MASS 2 
MATA( 10 , 19 ) = -1.0 

SUM OF FORCES IN THE Y DIRECTION 
MATAlll ,11) = 1.0 
MATAlll, 14) = MASS 2 
MATA( 11 , 20 ) = -1.0 

SUM OF FORCES IN THE Z DIRECTION 
MATAI 12 ,12) =1.0 
MATAI 12 , 15) = MASS2 
MATA(12,2l) = -1.0 

SUM OF FORCES LINK TWO EQUAL 
MATB (12) = -W2 

EQUATIONS AT JOINT ONE 
IN THE X DIRECTION 



MATA( 13 , 4) 
MATAI 13 , 8) 
MATAI 13,9) 
MATAI 13, 13 
MATAI 13, 17 
MATA( 13 , 18 



= - 1.0 

= - RAG 1(3) 

= RAG1 (2 ) 

) = 1.0 

) = RBG2 (3) 

) = -RBG2(2) 



MATB ( 13 ) = MIC1 - MIC2 
IN THE Y DIRECTION 



MATA(14,5] 
MATAI 14, 7) 
MATA (14, 9) 
MATAI14, 14) 
MATA (14, 16 
MATA(l4,18’ 



= - 1.0 
= RAG1 (3) 

= -RAG1(1) 

= 1.0 

= -RBG2(3) 
= RBG2 ( 1 ) 



MATB (14) = MJC1 - MJC2 

IN THE Z DIRECTION 
MATA (15, 6) = -1.0 
MATA (15,7) = -RAG1 (2) 
MATA( 15 , 8 ) = RAG 1(1) 
MATA(15 ,15) = 1.0 
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X- X- X- X- 4^ X* X* 



MATA( 15 , 16 ) = RBG2(2) 
MATA ( 15 , 17 ) = -RBG2 ( 1 ) 



MATB (15) = MKC1 - MKC2 



* SUM OF MOMENTS EQUATIONS FOR LINK TWO IN THE X,Y,Z DIRECTIONS 
MATA ( 16 , 11 ) = RBG2(3) 

MATA( 16 , 12 ) = -RBG2(2) 

MATA (16,16) = - ( IXX(2 . 1 ) + IXX(2.2)) 

MATA (16,17) = IXY(2, 1) + IXY(2,2) 

MATAl 16 , 18) = I XZ ( 2 , 1 ) + IXZ(2,2) 

MATA (16, 20) = -RAG2(3) 

MATA (16.21) = RAG2 ( 2 ) 

MATB (16) = -T1X + T2X 
IF (RUN .EQ. 2) GO TO 11 



MATA( 17 , 10 ) 
MATA( 17 , 12 ) 
MATA(l7 , 16) 
MATA ( 17 , 17) 
MATA ( 17, 18) 
MATA ( 17 , 19 ) 
MATA ( 17, 21 ) 



-RBG2 ( 3 ) 
RBG2 ( 1 ) 
IXY(2 , 1 ) + 
-(IYY(2 1) 
IYZ(2,1) + 
RAG2(3 ) 
-RAG2 ( 1 ) 



IXY(2 ,2) 

+ IYY(2, 2 ) ) 
IYZ(2 ,2) 



MATB (17) = - T1Y + T2Y 



MATA(18, 10) 
MATA( 18 , 11 ) 
MATA( 18 , 16 ) 
MATA( 18 , 17 ) 
MATA( 18 , 18) 
MATA( 18 , 19 ) 
MATA(18,20) 



RBG2 ( 2 ) 
-RBG2 (1 ] 
IXZ(2 , 1 


! ♦ 


IYZ(2,lI 


) + 


- (IZZ(2 . 


■ l) 


-RAG2(2, 


RAG2 ( 1 ) 





IXZ(2 ,2) 
IYZ(2,2) 

+ IZZ(2 , 2) ) 



MATB ( 18) = - T1Z + T2Z 
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IF (RUN .EQ. 3) GO TO 4 
MATA( 17 , 17 ) = 1.0 
MATA(18, 18) = 1.0 



LINK THREE 

SUM OF FORCES IN THE X DIRECTION 
MATA( 19,19) = 1.0 

MATA(l9 ,22) = MASS3 

SUM OF FORCES IN THE Y DIRECTION 
MATA (20 , 20) = 1.0 
MATA (20 , 23 ) = MASS3 

SUM OF FORCES IN THE Z DIRECTION 
MATA (21 , 21 ) = 1.0 
MATA(21 ,24) = MASS3 

MATB (21) = -W3 

EQUATIONS AT JOINT TWO 
IN THE X DIRECTION 

MATA(22, 13) = -1.0 
MATA(22 , 17 ) = -RAG2(3) 
MATA(22 , 18) = RAG2(2) 

MATA(22 , 22) = 1.0 
MATA( 22,26) = RBG3(3) 
MATA(22,27) = -RBG3(2) 

MATB (22) = MIC3 - MIC4 

* IN THE Y DIRECTION 

MATA(23 , 14) = -1.0 
MATA(23 , 16) = RAG2(3) 
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MATA ( 23 , 18) = -RAG2(1) 

MATA(23,23) = 1.0 
MATA (23, 25) = -RBG3(3) 

MATA(23 , 27 ) = RBG3(1) 

MATB(23) = MJC3 - MJC4 

* IN THE Z DIRECTION 

MATA (24, 15) = -1.0 
MATA( 24 , 16 ) = -RAG2(2) 

MATA (24, 17) = RAG2(1) 

MATA (24, 24) =1.0 
MATA (24, 25) = RBG3(2) 

MATA (24, 26) = -RBG3(1) 

MATB(24) = MKC3 - MKC4 

* SUM OF MOMENTS EQUATIONS FOR LINK THREE IN THE X,Y,Z DIRECTIONS 

MATA (25, 20V = RBG3(3) 

MATA (25 , 21 ) = -RBG3(2) 

MATA (25 , 25 ) = -(IXX(3,1) + IXX(3.2)) 

MATA (25 ,26) = IXY(3,l) + IXY(3,2) 

MATA ( 25 , 27 ) = IXZ(3,l) + IXZ(3,2) 

MATB( 25 ) = ~ T2X 

I F ( RUN .EQ. 1 .OR. RUN .EQ. 2) GO TO 12 

MATA (26 ,19) = -RBG3(3) 

MATA (26 , 21 ) = RBG3 ( 1 ) 

MATA (26 , 25 ) = I XY ( 3 , 1 ) + IXY(3,2) 

MATA (26 , 26 ) = - ( IYY (3.1) + IYY(3,2)) 

MATA(26 , 27 ) = IYZ(3 , 1 ) + IYZ(3,2) 

MATB(26) = - T2Y 

MATA (27 , 19) = RBG3(2) 

MATA(27,20) = -RBG3 ( 1 ) 

MATA (27 , 25 ) = IXZ(3 , 1 ) + IXZ(3,2) 

MATA (27 , 26 ) = IYZ(3,l) + IYZ(3,2) 

MATA (27 ,27) = -(IZZ(3,1) + IZZ(3,2)) 

MATB(27 ) = - T2Z 

IF (RUN .EQ. 3) GO TO 13 
12 MATA (26 ,26) = 1.0 

MATA (27 ,27) = 1.0 



* CALL EQUATION SOLVER PROGRAM FROM IMSL 

13 CALL LEQT2F(MATA,M,N , IA ,MATB , IDGT , WKAREA , IER) 

IF (IER .NE. 0) CALL END JOB 

* FIND LCOGX , LCOGY , LCOGZ , THETA VALUES ,WX,WY,WZ 

IF (RUN .EQ. 1) GO TO 6 
IF (RUN .EQ. 2) GO TO 9 

* LINK ONE 

AX1 = MATB(4) 

VELX1 = INTGRL(0 ,AX1 ) 

LCOGX1 = INTGRL(X1,VELX1) 

LCOGX ( 1 ) = LCOGX1 
AY1 = MATB(5) 

VELY1 = INTGRL(0 ,AY1) 

LCOGY 1 = INTGRL(Y1,VELY1) 

LCOGY ( 1 ) = LCOGY1 
AZ1 = MATB ( 6 ) 

VELZ1 = INTGRL ( 0 , AZ1 ) 

LCOGZ1 = INTGRL (Z1,VELZ1) 

LCOGZ(l) = LCOGZ1 
WD1X = MATB ( 7 ) 
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kD * 



W1X = INTGRL (0 , WD1X) 

THEXR1 = INTGRL(TY1,W1X) 

JX0= LCOGX(l) - L ( 1 , 1 ) * COS(TXl) 
WDX(l) = WD1X 
WX(1) = W1X 

CTHETX(l) = THEXR1 * RADEG 
ETHETY(l) = CTHETX(l) 

WD1Y = MATB(8) 

W1Y = INTGRL (0,WD1Y) 

THEYR1 = INTGRL ( 0. ,W1Y) 

JYO = LCOGY(l) - L(l , 1) * COS (THEXR1 ) 
WDY(l) = WD1Y 
WY(1) = W1Y 

CTHETY ( 1 ) = THEYR1 * RADEG 
WD1Z = MATB(9) 

W1Z = INTGRL(0 ,WD1Z) 

THEZR1 = INTGRL(0. ,W1Z) 

WDZ(l) = WD1Z 
WZ(1) = W1Z 

CTHETZ ( 1 ) = THEZR1 * RADEG 
ETHETZ(l) = 90.0 - CTHETX(l) 

ETHEZ1 = ETHETZ(l) * DEGRA 
JZO = LCOGZ(l) - L( 1 , 1 ) * COS (ETHEZ1 ) 

LINK TWO 

AX2 = MATB(13) 

VELX2 = INTGRL (0,AX2) 

LCOGX2 = INTGRL (X2,VELX2) 

LCOGX(2) = LCOGX2 
AY 2 = MATB(14) 

VELY2 = INTGRL (0,AY2) 

LCOGY2 = INTGRL(Y2 ,VELY2) 

LCOGY(2) = LCOGY2 
AZ2 = MATB(15) 

VELZ2 = INTGRL (0,AZ2) 

LCOGZ2 = INTGRL ( Z2 ,VELZ2) 

LCOGZ(2) = LCOGZ2 
WD2X = MATB(16) 

W2X = INTGRL (0,WD2X) 

THEXR2 = INTGRL (TY2 , W2X) 

JX1 = LCOGX(2) - L(2,l) * C0S(TX2) 
WDX(2) = WD2X 
WX(2) = W2X 

CTHETX(2) = THEXR2 * RADEG 
ETHETY(2) = CTHETX(2) 

WD2Y = MATB(17 ) 

W2Y = INTRGL(0,WD2Y) 

THEYR2 = INTGRL (0 . ,W2Y) 

JY1 = LCOGY(2) - L( 2 , 1 ) * COS(THEXR2) 
WDY ( 2 ) = WD2Y 
WY(2) = W2Y 

CTHETY(2) =THEYR2 * RADEG 
WD2Z = MATB( 18) 

W2Z = INTGRL (0,WD2Z) 

THEZR2 = INTGRL ( 0. ,W2Z) 

WDZ(2) = WD2Z 
WZ(2) = W2Z 

CTHETZ(2) = THEZR2 * RADEG 
ETHETZ(2) =90.0 - CTHETX(2) 

ETHEZ2 = ETHETZ (2 ) * DEGRA 
JZ1 = LCOGZ(2) - L (2 , 1 ) * COS(ETHEZ2) 

LINK THREE 
6 AX3 = MATB(22) 

VELX3 = INTGRL ( 0. ,AX3) 

LCOGX3 = INTGRL (X3,VELX3) 

LCOGX(3 ) = LCOGX3 
AY3 = MATB(23) 

VELY3 = INTGRL ( 0. ,AY3) 
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LC0GY3 = INTGRL ( Y3 , VELY3 ) 

LC0GY(3) = LC0GY3 
AZ3 = MATB(24) 

VELZ3 = INTGRL ( 0. ,AZ3) 

LCOGZ3 = INTGRL (Z3,VELZ3) 

LCOGZ ( 3 ) = LCOGZ3 
WD3X = MATB(25) 

W3X = INTGRL(0 . ,WD3X) 

THEXR3 = INTGRL(TY3,W3X) 

JX2 = LCOGX(3 ) - L (3 , 1 ) * COS(TX3) 
WDX(3) = WD3X 
WX(3) = W3X 

CTHETX(3) = THEXR3 * RADEG 
ETHETY(3) = CTHETX(3) 

WD3Y = MATB(26) 

W3Y = INTGRL ( 0. ,WD3Y) 

THEYR3 = INTGRL ( 0. ,W3Y) 

JY2 = LCOGY(3 ) - L(3,l) * COS(THEXR3) 
WDY ( 3 ) = WD3Y 
WY(3 ) = W3Y 

CTHETY(3) = THEYR3 * RADEG 
WD3Z = MATB(27) 

W3Z = INTGRL ( 0. ,WD3Z) 

THEZR3 = INTGRL ( 0. ,W3Z) 

WDZ(3) = WD3Z 
WZ(3 ) = W3Z 

CTHETZ (3 ) = THEZR3 * RADEG 
ETHETZ(3) = 90.0 - CTHETX(3) 

ETHEZ3 = ETHETZ(3) * DEGRA 

JZ2 = LCOGZ(3) - L(3 , 1 ) * COS(ETHEZ3) 

DYNAMIC 



THEORY ( 3 ) = ( (( (-2.5/(PI*PI))*SIN(W*TIME))+(5.*TIME)/PI)/IXXA(3)) . . 
+ PI/4. 

EULORY(3) = THEORY ( 3 ) * RADEG 
THDDOT(3) = T2X/IXXA(3) 

ERROR(3) = ( (ABS (EULORY(3 ) -ETHETY(3 ) ) )/ 81.476)*100. 



END 

STOP 

FORTRAN 

* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTOR 

SUBROUTINE CPROD ( VECTA , VECTB , MI , M J , MK) 

IMPLICIT REAL*8 (A-Z) 

DIMENSION VECTA(3) ,VECTB(3) 

MI = VECTA( 2 ) * VECTB (3) - VECTA(3) * VECTB(2) 

MJ = VECTA(3) * VECTB(l) - VECTA(l) * VECTB (3) 

MK = VECTA(l) * VECTB (2) - VECTA(2) * VECTB(l) 

RETURN 
END 
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APPENDIX B 



SIMULATION PROGRAM FOR MOVEMENT OF LINK TWO AND 

THREE 



TERMINAL 
METHOD ADAMS 

PRINT . 03 , T1X , T0RY1X , T2X , TORY2X , ERRT2X , ERRT1X, ETHETY( 2-3 ) 

CONTROL FINTIM =3.0, DELMAX =.01, DELPRT = .03 

SAVE . 01 , ERRT2X , ERRT1X , T0RY1X , TORY2X , T1X , T2X , ETHETY ( 2 ) , ETHETY( 3 ) 

GRAPH (DE=TEK6 18) TIME , T1X ,TORYlX 
GRAPH ( DE=TE K6 1 8 ) TIME , T2X , TORY2X 
GRAPH ( DE=TEK6 1 8 ) TIME , ERRT2X 
GRAPH (DE=TEK618) TIME , ERRT1X 
GRAPH(DE=TEK618) TIME ,ETHETY(3) , ETHETY(2) 

D DIMENSION MATA (27,27), MASS (3,2),L(3.2),RX(3,2),RY(3,2),RZ(3,2) 

D DIMENSION IXX(3 , 2) , IXZ(3 , 2) , IXY(3 , 2) , IYY(3 , 2) , IYZ(3 , 2) , IZZ(3 , 2) 

D INTEGER IER , I , RUN ,M ,N , IA , IDGT 

EXCLUDE IA, IDGT, IER, I ,RUN,M,N 

ARRAY MATB( 27 ) ,LCOGX(3 ) , LCOGY(3 ) , LCOGZ ( 3 ) , ETHETX ( 3 ) , ETHETY ( 3 ) , ETHETZ ( 3 ) 
ARRAY CTHETX ( 3 ) , CTHETY ( 3 ) , CTHETZ ( 3 ) 

ARRAY VECTAO ( 3 ) , VECTBO ( 3 ) . VECTA1 ( 3 ) , VECTB1 ( 3 ) , VECTA2 ( 3 ) , VECTB2 ( 3 ) 

ARRAY WDX( 3 ) ,WDY(3 ) , WDZ( 3 ) , WX( 3 ) , WY( 3) . WZ(3 ) ,RBG1 (3 ) , RAG1 (3 ) 

ARRAY RBG2(3) ,RAG2(3) , RBG3 ( 3 ) , THETXR ( 3 ) , THETYR ( 3 ) , THETZR(3) 

ARRAY SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 

D DATA MATA/729 * 0./ 

INITIAL 

* INPUT PARAMETER CONSTANTS 
A = 2.0 
P = 0.0 
W = 2*PI 
IDGT = 4 
G=0 . 0 
N=27 
M=1 

IA =27 
RUN = 2 



* INPUT JOINT LOCATIONS IN METERS 

JXO =0.0 
JYO = 0.0 
JZO = 0.0 
JX1 = 0.0 
JY1 = 1.0 
JZ1 = 0.0 
JX2 = 0.0 
JY2 = 2.0 
JZ2 = 0.0 

* INPUT TORQUE CONSTANTS 

TOX =0.0 
TOY =0.0 
TOZ =0.0 
T1Y = 0.0 
T1Z = 0.0 
T2Y =0.0 
T2Z =0.0 

*INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS FOR EACH LINK ENDS 



1,1) 


= 0.50 


1 ,2) 


= 0.50 


2,1) 


= 0.50 


2,2 


= 0.50 
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L(3 , 1 ) = 0.50 
L(3,2) = 0.50 

* INPUT MASS AT LINK ENDS IN KILOGRAMS 



MASS < 


11 , 1 ) 


= 


2.5 


MASS < 


1,2) 


= 


2.5 


MASS! 


2,1) 


= 


2.5 


MASS < 


2,2) 


= 


2.5 


MASS < 


3,1 




2.5 


MASS { 


3,2 


= 


2.5 



* INPUT OMEGA AND OMEGA DOT 



DO 30 I = 


1,3 


WX( I ) 


= 0.0 


WY ( I ) 


= 0.0 


WZ(I) 


= 0.0 


WDX(I) 


I = 0.0 


WDY ( I ) 


1 = 0.0 


WDZ(I) 


1 = 0.0 



30 CONTINUE 

* INPUT INITIAL VALUES OF EULER ANGLE THETA AND CONVERT TO RADIANS 

ETHETK(l) =90.0 
TK1 = ETHETX(l) * DEGRA 
ETHETY ( 1 ) =0.0 
TY1 = ETHETY ( 1 ) * DEGRA 
ETHETZ ( 1 ) =90.0 
TZ1 = ETHETZ(l) * DEGRA 
ETHETX(2) = 90.0 
TX2 = ETHETX(2) * DEGRA 
ETHETY(2) = 0.0 
TY2= ETHETY(2) * DEGRA 
ETHETZ (2) = 90.0 
TZ2 = ETHETZ (2) * DEGRA 
ETHETX(3) = 90.0 
TX3 = ETHETX(3) * DEGRA 
ETHETY (3) = 45.0 
TY3 = ETHETY(3) * DEGRA 
ETHETZ (3) = 45.0 
TZ3 = ETHETZ (3) * DEGRA 

* INPUT LOCATION OF LINK CENTERS OF GRAVITY 

LCOGX(l) =0.0 
XI = LCOGX(l) 

LCOGY(l) =0.5 
Y1 = LCOGY(l) 

LCOGZ(l) =0.0 
Z1 = LCOGZ(l) 

LCOGX( 2) = 0.0 
X2 = LCOGX(2) 

LCOGY(2) =1.5 
Y2 = LCOGY(2 ) 

LCOGZ(2) = 0.0 
Z2 = LCOGZ(2) 

LCOGX(3) =0.0 
X3 = LCOGX(3) 

THERAD = ETHETY(3) * DEGRA 
LCOGY(3) = 2.0 + COS (THERAD) * L(3,l) 

Y3 = LCOGY(3) 

LCOGZ(3) = L(3,l) * SIN (THERAD) 

Z3 = LCOGZ(3) 

* INPUT MASS OF EACH LINK IN KG AND COMPUTE WEIGHTS IN NEWTONS 

MAS SI =5.0 
MASS2 =5.0 
MASS3 =5.0 
W1 = MASSING 
W2 = MASS2*G 
W3 = MASS3*G 
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INPUT ACCELERATION OF JOINT ZERO 
AOX = 0.0 
AOY =0.0 
AOZ =0.0 



DERIVATIVE 

* INPUT JOINT EQUATIONS 

NOSORT 

* INITIALIZE MATRIX B TO ZERO 

DO 10 I = 1,27 
MATB(I) = 0.0 
10 CONTINUE 

* INPUT TORQUE AT JOINTS 

T2X = -A*SIN (W*TIME +P) 

T1X = A*SIN (W*TIME + P) 

* JOINT ZERO AB = AG1 + (WD1 X RB/G1 ) + W1 X (W1 X RB/G1) 

VECTAO ( 1 ) = WDX(l) 

VECTAO (2 ) = WDY(l) 

VECTAO ( 3 ) = WDZ(l) 

RBG1(1) = JXO - LCOGX(l) 

RBG1 ( 2 ) = JYO - LCOGY(l) 

RBG1 (3 ) = JZO - LCOGZ(l) 

CALL CPROD (VECTAO ,RBG1 ,MIA0 ,MJA0 ,MKA0 ) 

VECTAO ( 1 ) = WX(1) 

VECTAO (2) = WY(1) 

VECTAO (3) = WZ(1) 

CALL CPROD (VECTAO , RBG1 , MIBO ,MJB0 , MKBO ) 

VECTBO ( 1 ) = MIBO 
VECTBO (2 ) = MJBO 
VECTBO (3) = MKBO 

CALL CPROD (VECTAO , VECTBO , MI CO , MJCO , MKCO ) 

* JOINT ONE EQUATIONS--- AA = AG1 + (WD1 X RA/G1) + W1 X (W1 X RA/G1 ) 

VECTA1 ( 1 ) = WDX(l) 

VECTA1(2) = WDY(l) 

VECTA1 ( 3 ) = WDZ(l) 

RAG1(1) = JX1 - LCOGX(l) 

RAG1 (2 ) = JY1 - LCOGY(l) 

RAG1 ( 3 ) = JZ1 - LCOGZ(l) 

CALL CPROD ( VECTA1 , RAG1 , MIA1 , MJA1 , MKA1 ) 

VECTA1 ( 1 ) = WX(1) 

VECTA1 ( 2 ) = WY(1) 

VECTA1 ( 3 ) = WZ(1) 

CALL CPROD (VECTA1 , RAG1 ,MIB1 ,MJB1 ,MKB1 ) 

VECTB1 ( 1 ) = MIB1 
VECTB1 (2) = MJB1 
VECTB1 (3) = MKB1 

CALL CPROD (VECTA1 , VECTB1 ,MIC1 ,MJC1 ,MKC1 ) 

* AB = AG2 + (WD2 X RB/G2) + W2 X (W2 X RB/G2) 

VECTA1 ( 1 ) = WDX(2) 

VECTA1 (2) = WDY ( 2 ) 

VECTA1 ( 3 ) = WDZ(2) 

RBG2 (1 ) = JX1 - LCOGX(2) 

RBG2 ( 2 ) = JY1 - LCOGY ( 2 ) 

RBG2 ( 3 ) = JZ1 - LCOGZ ( 2 ) 

CALL CPROD (VECTA1 , RBG2 ,MIA2 , MJA2 ,MKA2 ) 

VECTA1 ( 1 ) = WX(2) 

VECTA1 ( 2 ) = WY ( 2 ) 

VECTA1 ( 3 ) = WZ(2) 

CALL CPROD (VECTA1 , RBG2 ,MIB2 ,MJB2 ,MKB2) 

VECTB1 ( 1 ) = MIB2 
VECTB1 (2 ) = MJB2 
VECTB1 ( 3 ) = MKB2 
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CALL CPROD (VECTA1 , VECTB1 ,MIC2 , MJC2 ,MKC2 ) 



* JOINT TWO EQUATIONS 

* AA = AG2 + (WD2 X RA/G2) + W2 X (W2 X RA/G2) 

VECTA2(1) = WDX( 2 ) 

VECTA2(2) = WDY(2) 

VECTA2(3) = WDZ ( 2 ) 

RAG2 ( 1 ) = JX2 - LCOGX(2) 

RAG2 ( 2 ) = JY2 - LCOGY(2) 

RAG2(3) = JZ2 - LCOGZ ( 2 ) 

CALL CPROD (VECTA2 , RAG2 ,MIA3 ,MJA3 ,MKA3 ) 
VECTA2 (1 ) = WX(2) 

VECTA2 ( 2 ) = WY ( 2 ) 

VECTA2(3) = WZ(2) 

CALL CPROD (VECTA2 , RAG2 ,MIB3 ,MJB3 ,MKB3 ) 
VECTB2 ( 1 ) = MIB3 
VECTB2(2) = MJB3 
VECTB2 (3) = MKB3 

CALL CPROD (VECTA2 , VECTB2 , MIC3 , MJC3 ,MKC3 ) 

* AB = AG3 + (WD3 X RB/G3) + W3 X (W3 X RB/G3 ) 

VECTA2 ( 1 ) = WDX ( 3 ) 

VECTA2(2) = WDY ( 3 ) 

VECTA2(3) = WDZ(3) 

RBG3(1) = JX2 - LCOGX(3) 

RBG3 (2) = JY2 - LCOGY(3) 

RBG3(3) = JZ2 - LCOGZ(3) 

CALL CPROD (VECTA2 , RBG3 ,MIA4,MKA4,MKA4) 
VECTA2 ( 1 ) = WX(3) 

VECTA2 (2) = WY(3) 

VECTA2 ( 3 ) = WZ(3) 

CALL CPROD (VECTA2 , RBG3 ,MIB4 ,MJB4 ,MKB4) 
VECTB2 ( 1 ) = MIB4 
VECTB2 ( 2 ) = MJB4 
VECTB2 ( 3 ) = MKB4 

CALL CPROD ( VECTA2 , VECTB2 , MIC4 , MJC4 , MKC4 ) 

* SUM OF MOMENTS EQUATIONS 

* CONVERT EULER ANGLES FROM DEGREES TO RADIANS 

DO 40 I = 1,3 

THETXR(I) = ETHETX(I) * DEGRA 

THETYR(I) = ETHETY(I) * DEGRA 

THETZR(I) = ETHETZ(I) * DEGRA 



* 



COMPUTE HX,H DOT X,HY,H 



RX ( I , 1 ) = -L( 
RX( I , 2 ) = L(I 
RY ( I , 1 ) = -L( 
RY ( I , 2 ) = L(I 
RZ ( I , 1 ) = -L ( 
RZ ( 1 , 2 ) = L(I 
IXX(I , 1 )=MASS(I , 1'* 
IXX( I , 2 )=MASS (1,2 
I XZ ( 1 , 1 ) =MAS S ( 1 , 1 
IXZ ( I , 2 ) =MASS (1,2 
IXY(I , 1)=MASS( I , 1 
IXY ( I , 2 )=MASS (1,2 
HDX(l) = WDX( 1 
HDX(2) = WDX (2 
IYY(I , 1 ) = MASS ( I , 1 
IYY (1,2) = MASS (1,2 
IYZ( 1,1) = MASS (1,1 
IYZ(I , 2) = MASS (1.2 
HDY(l) = WDY ( I 
HDY ( 2 ) = WDY ( I 
IZZ(I,1) = MASS (1,1 
IZZ(I , 2 ) = MASS ( I . 2 
HDZ(l) = WDZ(I 
HDZ ( 2 ) = WDZ( I 



DOT Y. HZ ,H DOT Z 
1,1) * COS (THETXR( I 
,2) * COS (THETXR( I ) 
1,1) * COS (THETYR(I 
,2) * COS (THETYR(I ) 
1,1) * COS(THETZR(I 
,2) * COS (THETZR( I ) 



li 



RY(I , 1 )*RY(I , 1 ) )+(RZ (I , t , , 
RY(I,2)*RY(I,2))+(RZ(I,2)*RZ(I,2 



RZ ( I , 1 
■ 1,2 
1,1 



* rx(i,i; 

* RX(I,2 

* RY ( 1 , 1 

* RY(I,2; 



l)*RZ(I,ljjj 



,j-WDZ(I)*IXZ(I,l 



* RZ 

* RX 

* RX ( 1 , 2 

*IXx|l'2)-WDZ(i)*IXZ{i)2 

* ( (RX(I ,1)*RX(I,1)) + 

* ((RX(I 2)*RX 1,2 ) + 

* RY 1,1 * RZ 1,1 

* RY ( 1 , 2 ) * RZ ( 1 , 2 ) 

*IYY (1,1) -WDX(I ) *IXY (1,1 

I ,2)-WDX(I)*IXY(I ,2 
RX(I , 1 )*RX(I , 1 ) ) + 
RX(I,2)*RX(I,2)) + 

1.1) -WDX(I)*IXZ(I,1 

1.2) -WDX(I)*IXZ(I,2 



l;Rz(i' f i)*Rz(i,i)’ 

(RZ(I,2)*RZ(I,2): 



-wdy(i)*ixy(i,i; 

WDY(I)*IXY(I ,2\ 



i*IYY 

■k 
■k 

*IZZ( 
■*IZZ< 



-WDZ(I)*IYZ(I,i; 
-WDZ (I )*IYZ(I , 2] 
(RY (1,1) *RY (1,1))) 
(RY ( I , 2 ) *RY (1,2))) 
)-wdy(i)*iyz(i,i, 

) -WDY ( I ) *IYZ (1,2, 
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* * * * * (Jj*** 



SUMHDX(I) = HDX(l) + HDX(2) 

SUMHDY(I) = HDY(l) + HDY ( 2 ) 

SUMHDZ(l) = HDZ(l) + HDZ(2) 

40 CONTINUE 

* TEST TO SEE WHICH CONSTRAINT IS IN EFFECT 1 , 2 OR 



IF 


(RUN 


.EQ. 


i) 


GO 


TO 


i 


IF 


RUN 


.EQ. 


2) 


GO 


TO 


2 


IF 


(RUN 


.EQ. 


3) 


GO 


TO 


3 



* INITIAIIZE MATRIX ACCORDING TO CONSTRAINT 



1 


DO 60 I = 1,18 




MATA (1,1) = 1.0 


60 


CONTINUE 




GO TO 4 


2 


DO 70 I = 1,9 




MATA (1,1) = 1.0 


70 


CONTINUE 




GO TO 7 



ENTER CONSTANTS INTO MATRIX A 
LINK ONE 

SUM OF FORCES IN THE X DIRECTION 
MATA(l , 1) = 1.0 

MATA(1 ,4) = MAS SI 
MATA(1 , 10) = -1.0 

SUM OF FORCES IN Y DIRECTION 
MATA (2,2) = 1.0 
MATA(2 , 5) = MASS1 
MATA (2 ,11) = -1.0 

SUM OF FORCES IN Z DIRECTION 
MATA (3 , 3 ) = 1.0 
MATA ( 3 ; 6 ) = MASS1 
MATA (3 ,12) = -1.0 

SUM OF FORCES LINK ONE EQUAL 
MATB ( 3 ) = -W1 

EQUATIONS AT JOINT ZERO 
IN THE X DIRECTION 
MATA(4,4) =1.0 
MATA (4 , 8) = RBG1 (3) 

MATA(4 , 9) = -RBG1 (2) 

MATB ( 4 ) = AOX - MICO 

IN THE Y DIRECTION 
MATA ( 5 , 5 ) =1.0 
MATA 5,7) = -RBG1 ( 3 ) 

MATA (5 ,9) = RBG1(1) 

MATB ( 5 ) = AOY - MJCO 

IN THE Z DIRECTION 
MATA (6, 6) = 1.0 
MATA( 6 , 7 ) = RBG1 ( 2 ) 

MATA(6,8) = -RBG1(1) 

MATB (6) = AOZ - MKCO 

* SUM OF MOMENTS EQUATIONS FOR LINK ONE IN THE X,Y, 



MATA< 


[1,2) 


= RBG1 (3) 




MATA< 


7,3) 


= -RBG1 ( 2 ) 




MATA< 


7,7 


= -(IXX(l.l) 


+ IXX(1 2)) 


MATA< 


7, 8 


= IXY ( 1 , 1 j + 


IXY( 1,2) 


MATA< 


!7,9) 


= IXZ(1,1) + 


IXZ(1,2) 
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* * 



MATA (7 ,11) = -RAG1 (3) 

MATA (7 ,12) = RAG1 (2) 

MATB(7) = T1X - TOX 

MATA(8 , 1 ) = -RBG1 ( 3 ) 
MATA(8,3)= RBG1(1) 

MATA (8,7) = I XY ( 1 , 1 ) + 

MATA (8, 8) = -(IYY(l.l) 

MATA( 8 , 9 ) = IYZ( 1,1) + 
MATA(8, 10) = RAG1 ( 3 ) 
MATA(8,12) = -RAG1(1) 

MATB(8) = T1Y - TOY 

MATA (9 , 1 ) = RBG1 (2) 

MATA(9 , 2) = -RBG1(1) 

MATA (9,7) = IXZ(l,l) + 

MATA (9,8) = IYZ(1,1) + 

MATA (9 , 9 ) = -(IZZ(1,1) 

MATA (9 ,10) = -RAG1 (2) 

MATA (9,11) = RAG1(1) 

MATB ( 9 ) = T1Z - TOZ 

* LINK TWO 

* SUM OF FORCES IN X DIRECTION 

7 MATA( 10,10) = 1.0 

MATA (10, 13) = MASS2 
MATA( 10,19) = -1.0 

SUM OF FORCES IN THE Y DIRECTION 
MATA (11,11) = 1.0 
MATA ( 1 1,14) = MASS2 
MATA( 11,20) = -1.0 

SUM OF FORCES IN THE Z DIRECTION 
MATA( 12,12) = 1.0 
MATA( 12,15) = MASS 2 
MATA( 12 , 21 ) = -1.0 

SUM OF FORCES LINK TWO EQUAL 
MATB (12) = -W2 

EQUATIONS AT JOINT ONE 
IN THE X DIRECTION 
MATA( 13,4) = -1.0 
MATA( 13 , 8) = -RAG1 (3) 

MATA ( 13 , 9 ) = RAG 1(2) 

MATA (13, 13) = 1.0 
MATA( 13,17) = RBG2(3) 
MATA(13 , 18) = -RBG2(2) 

MATB (13) = MIC1 - MIC2 

IN THE Y DIRECTION 
MATA( 14,5) = -1.0 
MATA (14, 7 ) = RAG1 (3 ) 

MATA (14, 9) = -RAGl(l) 

MATA 14,14) = 1.0 
MATA( 14 , 16 ) = -RBG2(3) 

MATA ( 14 , 18) = RBG2 ( 1 ) 

MATB (14) = MJC1 - MJC2 

IN THE Z DIRECTION 
MATA( 15 , 6) = -1.0 
MATA( 15 , 7 ) = -RAG1 (2) 

MATA( 15 , 8) = RAG1(1) 



IXY (1,2) 

+ IYY(1 2)) 
IYZ( 1 ,2) 



IXZ(1 , 2 ) 
IYZ(1 ,2, 

+ IZZ( 1 , 2) ) 
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a- a- a- a- ►£> a- a- 



MATA(15 ,15) = 1.0 
MATA(15 , 16 ) = RBG2(2) 
MATA( 15 , 17 ) = -RBG2 ( 1 ) 

MATB(15) = MKC1 - HKC2 



* SUM OF MOMENTS EQUATIONS FOR LINK TWO IN THE X,Y, 
MATA(16 , 11 ) = RBG2(3) 

MATAl 16 , 12) = -RBG2 (2) 

MATA( 16 , 16 ) = - ( IXX(2 , 1 ) + IXX(2,2)) 

MATA( 16 , 17 ) = IXY (2,1) + IXY(2,2) 

MATA( 16,18) = IXZ(2,l) + IXZ(2,2) 

MATA( 16 , 20) = -RAG2 (3) 

MATA( 16,21) = RAG2( 2) 

MATB ( 16 ) = -T1X + T2X 
I F ( RUN .EQ. 2) GO TO 11 



MATA(17 , 10) 
MATA (17 , 12 ) 
MATA ( 17 , 16 ) 
MATA( 17 , 17 ) 
MATA( 17 ,18) 
MATA( 17 , 19 ) 
MATA(17 ,21) 



-RBG2 (3) 
RBG2 ( 1 ) 

IXY (2,1) + 
“ ( IYY(2 . 1) 
IYZ(2 , 1 ) + 
RAG2(3) 
-RAG2 ( 1 ) 



IXY (2 , 2 ) 

+ IYY(2 , 2) ) 
IYZ(2,2 



MATB (17 ) = - T1Y + T2Y 



MATA(18, 10) 
MATA( 18 , 11 ) 
MATA( 18 , 16 ) 
MATA( 18 , 17 ) 
MATA(18, 18) 
MATA(18, 19) 
MATA(18,20) 



RBG2 (2) 
-RBG2 ( 1 ) 
IXZ(2 , 1 ) 
IYZ( 2 , 1 ) 
-(IZZ 2 
-RAG2(2) 
RAG2 (1 ) 



+ 



+ 

1 ) 



IXZ(2,2) 

IYZ (2 , 2 ) 

+ IZZ(2,2) ) 



MATB (18) = - T1Z + T2Z 



IF (RUN .EQ. 3) GO TO 4 
11 MATA( 17 , 17) = 1.0 

MATA(18, 18) = 1.0 



LINK THREE 

SUM OF FORCES IN THE X DIRECTION 
MATA( 19,19) = 1.0 

MATA(19 , 22) = MASS3 



SUM OF FORCES IN THE Y DIRECTION 
MATA(20,20) = 1.0 
MATA(20 , 23) = MASS3 

SUM OF FORCES IN THE Z DIRECTION 
MATA( 21 , 21 ) =1.0 
MATA(21 , 24) = MASS3 

MATB ( 21 ) = -W3 
EQUATIONS AT JOINT TWO 

IN THE X DIRECTION 

MATA(22 ,13) = -1.0 
MATA(22 , 17 ) = -RAG2(3) 
MATA(22 ,18) = RAG2(2) 

MATA(22 , 22 ) = 1.0 
MATA (22 , 26 ) = RBG3(3) 

MATA(22 ,27 ) = -RBG3(2) 

MATB (22) = MIC3 - MIC4 

IN THE Y DIRECTION 

MATA( 23,14) = -1.0 
MATA( 23 , 16 ) = RAG2(3) 



DIRECTIONS 



73 



MATA(23 , 18 ) = -RAG2(1) 

MATA 23,23) = 1.0 
MATA 23,25) = -RBG3(3) 

MATA(23 , 27 ) = RBG3(1) 

MATB(23) = MJC3 - MJC4 

* IN THE Z DIRECTION 

MATA (24, 15) = -1.0 
MATA (24, 16) = -RAG2(2) 

MATA (24, 17) = RAG2(1) 

MATA (24, 24) =1.0 
MATA 24,25) = RBG3(2) 

MATA (24, 26) = -RBG3(1) 

MATB(24) = MKC3 - MKC4 

* SUM OF MOMENTS EQUATIONS FOR LINK THREE IN THE X,Y,Z DIRECTIONS 

MATA(25,20V = RBG3(3) 

MATA 25,21 ) = -RBG3(2) 

MATA(25,25) = -(I XX (3.1) + IXX(3.2)) 

MATA 25,26) = IXY(3,l) + IXY(3,2) 

MATA 25,27) = IXZ(3,l) + IXZ(3,2) 

MATB (25) = - T2X 

IF (RUN .EQ. 1 .OR. RUN .EQ. 2) GO TO 12 

MATA(26 , 19) = -RBG3(3) 

MATA (26 , 21 ) = RBG3(1) 

MATA (26 , 25 ) = IXY(3,1) + IXY(3,2) 

MATA(26 , 26 ) = -(IYY(3,1) + IYY(3,2)) 

MATA(26 , 27 ) = IYZ(3,l) + IYZ(3,2) 

MATB (26) = - T2Y 

MATA(27 ,19) = RBG3(2) 

MATA (27 ,20) = -RBG3(l) 

MATA (27 ,25) = IXZ(3,l) + IXZ(3,2) 

MATA (27 ,26) = IYZ(3,l) + IYZ(3,2) 

MATA (27 ,27) = -(IZZ(3,1) + IZZ(3,2)) 

MATB (27 ) = - T2Z 

IF (RUN .EQ. 3) GO TO 13 
12 MATA(26,26) = 1.0 

MATA(27 , 27 ) = 1.0 



* CALL EQUATION SOLVER PROGRAM FROM IMSL 

13 CALL LEQT2F (MATA,M ,N , IA ,MATB , IDGT , WKAREA, IER) 

IF (IER .NE. 0) CALL ENDJOB 

* FIND LCOGX , LCOGY , LCOGZ , THETA VALUES ,WX,WY,WZ 

IF (RUN .EQ. 1) GO TO 6 
IF (RUN .EQ. 2) GO TO 9 

* LINK ONE 

AX1 = MATB (4) 

VELX1 = INTGRL(0 , AX1 ) 

LCOGX1 = INTGRL ( XI , VELX1 ) 

LCOGX(l) = LCOGX1 
AY1 = MATB ( 5 ) 

VELY1 = INTGRL (0,AY1) 

LCOGY1 = INTGRL (Y1 ,VELY1) 

LCOGY ( 1 ) = LCOGY1 
AZ1 = MATB (6) 

VELZ1 = INTGRL (0,AZ1) 

LCOGZ1 = INTGRL (Z1,VELZ1) 

LCOGZ ( 1 ) = LCOGZ 1 
WD1X = MATB (7) 
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W1X = INTGRL ( 0 , WD1X) 

THEXR1 = INTGRL (TY1,W1X) 

JXO= LCOGX(l) - L(1 , 1) * COS(TXl) 
WDX(l) = WD1X 
WX(1) = W1X 

CTHETX(l) = THEXR1 * RADEG 
ETHETY ( 1 ) = CTHETX(l) 

WD1Y = MATB (8) 

W1Y = INTGRL (0,WD1Y) 

THEYR1 = INTGRL ( 0. ,W1Y) 

JYO = LCOGY(l) - L ( 1 , 1 ) * COS (THEXR1 ) 
WDY(l) = WD1Y 
WY ( 1 ) = W1Y 

CTHETY ( 1 ) = THEYR1 * RADEG 
WD1Z = MATB ( 9 ) 

W1Z = INTGRL (0,WD1Z) 

THEZR1 = INTGRL ( 0. ,W1Z) 

WDZ(l) = WD1Z 
WZ(1) = W1Z 

CTHETZ(l) = THEZR1 * RADEG 
ETHETZ(l) = 90.0 - CTHETX(l) 

ETHEZ1 = ETHETZ(l) * DEGRA 

JZO = LCOGZ(l) - L(l,l) * COS(ETHEZl) 

* LINK TWO 

9 AX2 = MATB (13) 

VELX2 = INTGRL ( 0. ,AX2) 

LCOGX2 = INTGRL (X2,VELX2) 

LCOGX( 2 ) = LCOGX2 
AY2 = MATB (14) 

VELY2 = INTGRL ( 0. ,AY2) 

LCOGY2 = INTGRL (Y2,VELY2) 

LCOGY ( 2 ) = LCOGY2 
AZ2 = MATB (15) 

VELZ2 = INTGRL ( 0. ,AZ2) 

LCOGZ2 = INTGRL (Z2,VELZ2) 

LCOGZ(2) = LCOGZ2 
WD2X = MATB (16) 

W2X = INTGRL ( 0. ,WD2X) 

THEXR2 = INTGRL (TY2,W2X) 

JX1 = LCOGX(2) - L(2, 1) * COS(TX2) 

WDX ( 2 ) = WD2X 
WX( 2 ) = W2X 

CTHETX(2) = THEXR2 * RADEG 
ETHETY (2) = CTHETX(2) 

WD2Y = MATB (17) 

W2Y = INTGRL ( 0. ,WD2Y) 

THEYR2 = INTGRL ( 0. ,W2Y) 

JY1 = LCOGY(2) - L(2 , 1 ) * COS(THEXR2) 
WDY(2) = WD2Y 
WY(2) = W2Y 

CTHETY(2) =THEYR2 * RADEG 
WD2Z = MATB (18) 

W2Z = INTGRL ( 0. ,WD2Z) 

THEZR2 = INTGRL ( 0. ,W2Z) 

WDZ(2) = WD2Z 
WZ(2) = W2Z 

CTHETZ(2) = THEZR2 * RADEG 
ETHETZ(2) = 90.0 - CTHETX(2) 

ETHEZ2 = ETHETZ(2) * DEGRA 

JZ1 = LCOGZ(2) - L(2 , 1) * COS(ETHEZ2) 

* LINK THREE 

6 AX3 = MATB (22) 

VELX3 = INTGRL ( 0. ,AX3) 

LCOGX3 = INTGRL (X3,VELX3) 

LCOGX( 3 ) = LCOGX3 
AY3 = MATB (23) 

VELY3 = INTGRL ( 0. ,AY3) 
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* X 



LC0GY3 = INTGRL ( Y3 , VELY3 ) 

LC0GY(3) = LC0GY3 
AZ3 = MATB(24) 

VELZ3 = INTGRL ( 0. ,AZ3) 

LCOGZ3 = INTGRL (Z3,VELZ3) 

LCOGZ(3) = LCOGZ3 
WD3X = MATB(25) 

W3X = INTGRL ( 0. ,WD3X) 

THEXR3 = INTGRL (TY3,W3X) 

JX2 = LCOGX(3) - L(3 , 1 ) * COS(TX3) 

WDX(3) = WD3X 
WX(3) = W3X 

CTHETX(3) = THEXR3 * RADEG 
ETHETY(3 ) = CTHETX(3) 

WD3Y = MATB(26) 

W3Y = INTGRL ( 0. ,WD3Y) 

THEYR3 = INTGRL ( 0. ,W3Y) 

JY2 = LCOGY(3 ) - L(3,l) * COS(THEXR3) 

WDY(3) = WD3Y 
WY(3 ) = W3Y 

CTHETY(3) = THEYR3 * RADEG 
WD3Z = MATB(27 ) 

W3Z = INTGRL ( 0. ,WD3Z) 

THEZR3 = INTGRL ( 0. ,W3Z) 

WDZ(3) = WD3Z 
WZ(3) = W3Z 

CTHETZ(3) = THEZR3 * RADEG 
ETHETZ(3) = 90.0 - CTHETX(3) 

ETHEZ3 = ETHETZ(3 ) * DEGRA 

JZ2 = LCOGZ(3) - L(3, 1) * COS(ETHEZ3) 

DYNAMIC 

* COMPUTE THEORITICAL TORQUE, T1X AND T2X 
Y = L(3 , 1 ) * COS (THEXR3) 

Z = L (3 , 1 ) * SIN(THEXR3) 

FZ2 =-MASS3*AZ3 
FY2 = -MASS3 * AY3 
FZ1 = FZ2 - MASS 2 * AZ2 
FY1 = FY2 - MASS 2 * AY2 

TORY2X = (MAS S3 * L(3,2)**2)*WDX(3) - (FZ2 * Y)+ (FY2 * Z) 
TORY IX = (MASS2*L(2, 1 )**2)*WDX(2 )+TORY2X-FZl*COS (THEXR2 ) . 
*L(2,l)+FYl*SIN(THEXR2)*L(2,l)-FZ2*L(2,2)*COS(THEXR2)+FY2 
*SIN(THEXR2)*L(2,2) 

COMPUTE ERROR BETWEEN COMPUTED AND INPUTEDVALUES OF TORQUE AT 
JOINT ONE AND TWO 

ERRT2X = ( (TORY2X-T2X)/ 4.7553) * 100. 

ERRT1X = ( (TORY1X-T1X)/ 4.7553) * 100. 

END 

STOP 

FORTRAN 

* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 

SUBROUTINE CPROD(VECTA,VECTB,MI ,MJ,MK) 

IMPLICIT REAL*8 (A-Z) 

DIMENSION VECTA(3) ,VECTB(3) 

MI = VECTA(2) * VECTB (3 ) - VECTA(3) * VECTB(2) 

MJ = VECTA( 3) * VECTB(l) - VECTA(l) * VECTB(3) 

MK = VECTA(l) * VECTB (2) - VECTA(2) * VECTB(l) 

RETURN 

END 
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